diff --git a/common/autoware_grid_map_utils/test/benchmark.cpp b/common/autoware_grid_map_utils/test/benchmark.cpp index 90545135b3e18..a63ed9985fef1 100644 --- a/common/autoware_grid_map_utils/test/benchmark.cpp +++ b/common/autoware_grid_map_utils/test/benchmark.cpp @@ -46,7 +46,7 @@ int main(int argc, char * argv[]) result_file << "#Size PolygonVertices PolygonIndexes grid_map_utils_constructor grid_map_utils_iteration " "grid_map_constructor grid_map_iteration\n"; - autoware_universe_utils::StopWatch stopwatch; + autoware::universe_utils::StopWatch stopwatch; constexpr auto nb_iterations = 10; constexpr auto polygon_side_vertices = diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp index 278ea3fbcae80..9c1f7c876e73e 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ #define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ -namespace autoware_motion_utils +namespace autoware::motion_utils { constexpr double overlap_threshold = 0.1; -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp index 4b4aec2f5b398..308528954eccc 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp @@ -22,12 +22,12 @@ #include #include -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp index fe8cab2b7a070..30eca6927db34 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp @@ -24,7 +24,7 @@ #include #include -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index 44cab2ed8b52b..4679e3d9aba91 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -21,7 +21,7 @@ #include -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index c2ceaddd16871..07fcbd9840c88 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp index be0a9680ee378..19328179932c4 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp @@ -21,7 +21,7 @@ #include -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp index f80088645243e..5d622c54da452 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp @@ -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 = autoware_motion_utils::calcArcLength(points); + const double points_length = autoware::motion_utils::calcArcLength(points); return points_length >= resampling_intervals.back(); } @@ -50,9 +50,9 @@ template bool validate_points_duplication(const T & points) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pt = autoware_universe_utils::getPoint(points.at(i)); - const auto & next_pt = autoware_universe_utils::getPoint(points.at(i + 1)); - const double ds = autoware_universe_utils::calcDistance2d(curr_pt, next_pt); + const auto & curr_pt = autoware::universe_utils::getPoint(points.at(i)); + const auto & next_pt = autoware::universe_utils::getPoint(points.at(i + 1)); + const double ds = autoware::universe_utils::calcDistance2d(curr_pt, next_pt); if (ds < close_s_threshold) { return false; } @@ -67,27 +67,27 @@ bool validate_arguments(const T & input_points, const std::vector & resa // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { RCLCPP_DEBUG( get_logger(), "invalid argument: The number of resampling intervals is less than 2"); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points"); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } @@ -100,23 +100,23 @@ bool validate_arguments(const T & input_points, const double resampling_interval // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } // check resampling interval - if (resampling_interval < autoware_motion_utils::overlap_threshold) { + if (resampling_interval < autoware::motion_utils::overlap_threshold) { RCLCPP_DEBUG( get_logger(), "invalid argument: resampling interval is less than %f", - autoware_motion_utils::overlap_threshold); - autoware_universe_utils::print_backtrace(); + autoware::motion_utils::overlap_threshold); + autoware::universe_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp index c28f76553f71e..d4f88c17c4d70 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp @@ -23,7 +23,7 @@ #include -namespace autoware_motion_utils +namespace autoware::motion_utils { using TrajectoryPoints = std::vector; @@ -115,6 +115,6 @@ inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( return output; } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp index 18dbefa85918b..5272478cccd78 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_motion_utils +namespace autoware::motion_utils { /** * @brief An interpolation function that finds the closest interpolated point on the trajectory from @@ -73,24 +73,24 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar } if (points.size() < 2 || target_length < 0.0) { - return autoware_universe_utils::getPose(points.front()); + return autoware::universe_utils::getPose(points.front()); } double accumulated_length = 0; for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pose = autoware_universe_utils::getPose(points.at(i)); - const auto & next_pose = autoware_universe_utils::getPose(points.at(i + 1)); - const double length = autoware_universe_utils::calcDistance3d(curr_pose, next_pose); + const auto & curr_pose = autoware::universe_utils::getPose(points.at(i)); + const auto & next_pose = autoware::universe_utils::getPose(points.at(i + 1)); + const double length = autoware::universe_utils::calcDistance3d(curr_pose, next_pose); if (accumulated_length + length > target_length) { const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6); - return autoware_universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); + return autoware::universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); } accumulated_length += length; } - return autoware_universe_utils::getPose(points.back()); + return autoware::universe_utils::getPose(points.back()); } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp index 43c8a14d0f3a3..0d875e636bad5 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -20,7 +20,7 @@ #include #include -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 3c6539f027216..da37d04550f5e 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -35,7 +35,7 @@ #include #include -namespace autoware_motion_utils +namespace autoware::motion_utils { static inline rclcpp::Logger get_logger() { @@ -51,7 +51,7 @@ template void validateNonEmpty(const T & points) { if (points.empty()) { - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); throw std::invalid_argument("[autoware_motion_utils] validateNonEmpty(): Points is empty."); } } @@ -72,22 +72,22 @@ extern template void validateNonEmpty void validateNonSharpAngle( const T & point1, const T & point2, const T & point3, - const double angle_threshold = autoware_universe_utils::pi / 4) + const double angle_threshold = autoware::universe_utils::pi / 4) { - const auto p1 = autoware_universe_utils::getPoint(point1); - const auto p2 = autoware_universe_utils::getPoint(point2); - const auto p3 = autoware_universe_utils::getPoint(point3); + const auto p1 = autoware::universe_utils::getPoint(point1); + const auto p2 = autoware::universe_utils::getPoint(point2); + const auto p3 = autoware::universe_utils::getPoint(point3); const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - const auto dist_1to2 = autoware_universe_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = autoware_universe_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware::universe_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = autoware::universe_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); throw std::invalid_argument( "[autoware_motion_utils] validateNonSharpAngle(): Too sharp angle."); } @@ -106,10 +106,10 @@ std::optional isDrivingForward(const T & points) } // check the first point direction - const auto & first_pose = autoware_universe_utils::getPose(points.at(0)); - const auto & second_pose = autoware_universe_utils::getPose(points.at(1)); + const auto & first_pose = autoware::universe_utils::getPose(points.at(0)); + const auto & second_pose = autoware::universe_utils::getPose(points.at(1)); - return autoware_universe_utils::isDrivingForward(first_pose, second_pose); + return autoware::universe_utils::isDrivingForward(first_pose, second_pose); } extern template std::optional @@ -135,10 +135,10 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) return std::nullopt; } if (points_with_twist.size() == 1) { - if (0.0 < autoware_universe_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 < autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { return true; } - if (0.0 > autoware_universe_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 > autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { return false; } return std::nullopt; @@ -182,8 +182,8 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) constexpr double eps = 1.0E-08; for (size_t i = start_idx + 1; i < points.size(); ++i) { - const auto prev_p = autoware_universe_utils::getPoint(dst.back()); - const auto curr_p = autoware_universe_utils::getPoint(points.at(i)); + const auto prev_p = autoware::universe_utils::getPoint(dst.back()); + const auto curr_p = autoware::universe_utils::getPoint(points.at(i)); if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } @@ -299,7 +299,7 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto dist = autoware_universe_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { min_dist = dist; min_idx = i; @@ -351,13 +351,13 @@ std::optional findNearestIndex( size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto squared_dist = autoware_universe_utils::calcSquaredDistance2d(points.at(i), pose); + const auto squared_dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose); if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) { continue; } - const auto yaw = autoware_universe_utils::calcYawDeviation( - autoware_universe_utils::getPose(points.at(i)), pose); + const auto yaw = autoware::universe_utils::calcYawDeviation( + autoware::universe_utils::getPose(points.at(i)), pose); if (std::fabs(yaw) > max_yaw) { continue; } @@ -409,7 +409,7 @@ double calcLongitudinalOffsetToSegment( "[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(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -437,7 +437,7 @@ double calcLongitudinalOffsetToSegment( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + ": Longitudinal offset calculation is not supported for the same points."); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -448,8 +448,8 @@ double calcLongitudinalOffsetToSegment( return std::nan(""); } - const auto p_front = autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_front = autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; @@ -599,7 +599,7 @@ double calcLateralOffset( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -614,8 +614,8 @@ double calcLateralOffset( const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices; const auto p_back_idx = p_front_idx + 1; - const auto p_front = autoware_universe_utils::getPoint(overlap_removed_points.at(p_front_idx)); - const auto p_back = autoware_universe_utils::getPoint(overlap_removed_points.at(p_back_idx)); + const auto p_front = autoware::universe_utils::getPoint(overlap_removed_points.at(p_front_idx)); + const auto p_back = autoware::universe_utils::getPoint(overlap_removed_points.at(p_back_idx)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; @@ -672,7 +672,7 @@ double calcLateralOffset( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -724,7 +724,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t double dist_sum = 0.0; for (size_t i = src_idx; i < dst_idx; ++i) { - dist_sum += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); } return dist_sum; } @@ -772,7 +772,7 @@ std::vector calcSignedArcLengthPartialSum( double dist_sum = 0.0; partial_dist.push_back(dist_sum); for (size_t i = src_idx; i < dst_idx - 1; ++i) { - dist_sum += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); partial_dist.push_back(dist_sum); } return partial_dist; @@ -959,10 +959,10 @@ std::vector calcCurvature(const T & points) } for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = autoware_universe_utils::getPoint(points.at(i - 1)); - const auto p2 = autoware_universe_utils::getPoint(points.at(i)); - const auto p3 = autoware_universe_utils::getPoint(points.at(i + 1)); - curvature_vec.at(i) = (autoware_universe_utils::calcCurvature(p1, p2, p3)); + const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); + const auto p2 = autoware::universe_utils::getPoint(points.at(i)); + const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); + curvature_vec.at(i) = (autoware::universe_utils::calcCurvature(p1, p2, p3)); } curvature_vec.at(0) = curvature_vec.at(1); curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); @@ -997,13 +997,13 @@ std::vector> calcCurvatureAndArcLength(const T & point std::vector> curvature_arc_length_vec; curvature_arc_length_vec.emplace_back(0.0, 0.0); for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = autoware_universe_utils::getPoint(points.at(i - 1)); - const auto p2 = autoware_universe_utils::getPoint(points.at(i)); - const auto p3 = autoware_universe_utils::getPoint(points.at(i + 1)); - const double curvature = autoware_universe_utils::calcCurvature(p1, p2, p3); + const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); + const auto p2 = autoware::universe_utils::getPoint(points.at(i)); + const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); + const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3); const double arc_length = - autoware_universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + - autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); curvature_arc_length_vec.emplace_back(curvature, arc_length); } curvature_arc_length_vec.emplace_back(0.0, 0.0); @@ -1078,7 +1078,7 @@ std::optional calcLongitudinalOffsetPoint( "[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(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1094,7 +1094,7 @@ std::optional calcLongitudinalOffsetPoint( } if (src_idx + 1 == points.size() && offset == 0.0) { - return autoware_universe_utils::getPoint(points.at(src_idx)); + return autoware::universe_utils::getPoint(points.at(src_idx)); } if (offset < 0.0) { @@ -1110,12 +1110,12 @@ std::optional calcLongitudinalOffsetPoint( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = autoware_universe_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return autoware_universe_utils::calcInterpolatedPoint( + return autoware::universe_utils::calcInterpolatedPoint( p_back, p_front, std::abs(dist_res / dist_segment)); } } @@ -1209,7 +1209,7 @@ std::optional calcLongitudinalOffsetPose( "[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(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1223,7 +1223,7 @@ std::optional calcLongitudinalOffsetPose( } if (src_idx + 1 == points.size() && offset == 0.0) { - return autoware_universe_utils::getPose(points.at(src_idx)); + return autoware::universe_utils::getPose(points.at(src_idx)); } if (offset < 0.0) { @@ -1236,12 +1236,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = reverse_points.at(i); const auto & p_back = reverse_points.at(i + 1); - const auto dist_segment = autoware_universe_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { - return autoware_universe_utils::calcInterpolatedPose( + return autoware::universe_utils::calcInterpolatedPose( p_back, p_front, std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1253,12 +1253,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = autoware_universe_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return autoware_universe_utils::calcInterpolatedPose( + return autoware::universe_utils::calcInterpolatedPose( p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1358,8 +1358,8 @@ std::optional insertTargetPoint( return {}; } - const auto p_front = autoware_universe_utils::getPoint(points.at(seg_idx)); - const auto p_back = autoware_universe_utils::getPoint(points.at(seg_idx + 1)); + const auto p_front = autoware::universe_utils::getPoint(points.at(seg_idx)); + const auto p_back = autoware::universe_utils::getPoint(points.at(seg_idx + 1)); try { validateNonSharpAngle(p_front, p_target, p_back); @@ -1369,9 +1369,9 @@ std::optional insertTargetPoint( } const auto overlap_with_front = - autoware_universe_utils::calcDistance2d(p_target, p_front) < overlap_threshold; + autoware::universe_utils::calcDistance2d(p_target, p_front) < overlap_threshold; const auto overlap_with_back = - autoware_universe_utils::calcDistance2d(p_target, p_back) < overlap_threshold; + autoware::universe_utils::calcDistance2d(p_target, p_back) < overlap_threshold; const auto is_driving_forward = isDrivingForward(points); if (!is_driving_forward) { @@ -1381,31 +1381,31 @@ std::optional insertTargetPoint( geometry_msgs::msg::Pose target_pose; { const auto p_base = is_driving_forward.value() ? p_back : p_front; - const auto pitch = autoware_universe_utils::calcElevationAngle(p_target, p_base); - const auto yaw = autoware_universe_utils::calcAzimuthAngle(p_target, p_base); + const auto pitch = autoware::universe_utils::calcElevationAngle(p_target, p_base); + const auto yaw = autoware::universe_utils::calcAzimuthAngle(p_target, p_base); target_pose.position = p_target; - target_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); + target_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } auto p_insert = points.at(seg_idx); - autoware_universe_utils::setPose(target_pose, p_insert); + autoware::universe_utils::setPose(target_pose, p_insert); geometry_msgs::msg::Pose base_pose; { const auto p_base = is_driving_forward.value() ? p_front : p_back; - const auto pitch = autoware_universe_utils::calcElevationAngle(p_base, p_target); - const auto yaw = autoware_universe_utils::calcAzimuthAngle(p_base, p_target); + const auto pitch = autoware::universe_utils::calcElevationAngle(p_base, p_target); + const auto yaw = autoware::universe_utils::calcAzimuthAngle(p_base, p_target); - base_pose.position = autoware_universe_utils::getPoint(p_base); - base_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); + base_pose.position = autoware::universe_utils::getPoint(p_base); + base_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } if (!overlap_with_front && !overlap_with_back) { if (is_driving_forward.value()) { - autoware_universe_utils::setPose(base_pose, points.at(seg_idx)); + autoware::universe_utils::setPose(base_pose, points.at(seg_idx)); } else { - autoware_universe_utils::setPose(base_pose, points.at(seg_idx + 1)); + autoware::universe_utils::setPose(base_pose, points.at(seg_idx + 1)); } points.insert(points.begin() + seg_idx + 1, p_insert); return seg_idx + 1; @@ -1539,9 +1539,9 @@ std::optional insertTargetPoint( const double target_length = insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); - const auto p_target = autoware_universe_utils::calcInterpolatedPoint( - autoware_universe_utils::getPoint(points.at(*segment_idx)), - autoware_universe_utils::getPoint(points.at(*segment_idx + 1)), ratio); + const auto p_target = autoware::universe_utils::calcInterpolatedPoint( + autoware::universe_utils::getPoint(points.at(*segment_idx)), + autoware::universe_utils::getPoint(points.at(*segment_idx + 1)), ratio); return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } @@ -1645,7 +1645,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - autoware_universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1687,9 +1687,9 @@ std::optional insertStopPoint( double accumulated_length = 0; for (size_t i = 0; i < points_with_twist.size() - 1; ++i) { - const auto curr_pose = autoware_universe_utils::getPose(points_with_twist.at(i)); - const auto next_pose = autoware_universe_utils::getPose(points_with_twist.at(i + 1)); - const double length = autoware_universe_utils::calcDistance2d(curr_pose, next_pose); + const auto curr_pose = autoware::universe_utils::getPose(points_with_twist.at(i)); + const auto next_pose = autoware::universe_utils::getPose(points_with_twist.at(i + 1)); + const double length = autoware::universe_utils::calcDistance2d(curr_pose, next_pose); if (accumulated_length + length + overlap_threshold > distance_to_stop_point) { const double insert_length = distance_to_stop_point - accumulated_length; return insertStopPoint(i, insert_length, points_with_twist, overlap_threshold); @@ -1749,7 +1749,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - autoware_universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1788,7 +1788,7 @@ 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 = autoware_motion_utils::insertTargetPoint( + const auto insert_idx = autoware::motion_utils::insertTargetPoint( stop_seg_idx, stop_point, points_with_twist, overlap_threshold); if (!insert_idx) { @@ -1796,7 +1796,7 @@ std::optional insertStopPoint( } for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { - autoware_universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return insert_idx; @@ -1836,8 +1836,8 @@ std::optional insertDecelPoint( for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { const auto & original_velocity = - autoware_universe_utils::getLongitudinalVelocity(points_with_twist.at(i)); - autoware_universe_utils::setLongitudinalVelocity( + autoware::universe_utils::getLongitudinalVelocity(points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity( std::min(original_velocity, velocity), points_with_twist.at(i)); } @@ -1860,30 +1860,30 @@ void insertOrientation(T & points, const bool is_driving_forward) { if (is_driving_forward) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & src_point = autoware_universe_utils::getPoint(points.at(i)); - const auto & dst_point = autoware_universe_utils::getPoint(points.at(i + 1)); - const double pitch = autoware_universe_utils::calcElevationAngle(src_point, dst_point); - const double yaw = autoware_universe_utils::calcAzimuthAngle(src_point, dst_point); - autoware_universe_utils::setOrientation( - autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware::universe_utils::getPoint(points.at(i)); + const auto & dst_point = autoware::universe_utils::getPoint(points.at(i + 1)); + const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); + autoware::universe_utils::setOrientation( + autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); if (i == points.size() - 2) { // Terminal orientation is same as the point before it - autoware_universe_utils::setOrientation( - autoware_universe_utils::getPose(points.at(i)).orientation, points.at(i + 1)); + autoware::universe_utils::setOrientation( + autoware::universe_utils::getPose(points.at(i)).orientation, points.at(i + 1)); } } } else { for (size_t i = points.size() - 1; i >= 1; --i) { - const auto & src_point = autoware_universe_utils::getPoint(points.at(i)); - const auto & dst_point = autoware_universe_utils::getPoint(points.at(i - 1)); - const double pitch = autoware_universe_utils::calcElevationAngle(src_point, dst_point); - const double yaw = autoware_universe_utils::calcAzimuthAngle(src_point, dst_point); - autoware_universe_utils::setOrientation( - autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware::universe_utils::getPoint(points.at(i)); + const auto & dst_point = autoware::universe_utils::getPoint(points.at(i - 1)); + const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); + autoware::universe_utils::setOrientation( + autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); } // Initial orientation is same as the point after it - autoware_universe_utils::setOrientation( - autoware_universe_utils::getPose(points.at(1)).orientation, points.at(0)); + autoware::universe_utils::setOrientation( + autoware::universe_utils::getPose(points.at(1)).orientation, points.at(0)); } } @@ -1908,14 +1908,14 @@ template void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { for (auto itr = points.begin(); std::next(itr) != points.end();) { - const auto p1 = autoware_universe_utils::getPose(*itr); - const auto p2 = autoware_universe_utils::getPose(*std::next(itr)); + const auto p1 = autoware::universe_utils::getPose(*itr); + const auto p2 = autoware::universe_utils::getPose(*std::next(itr)); const double yaw1 = tf2::getYaw(p1.orientation); const double yaw2 = tf2::getYaw(p2.orientation); if ( - max_yaw_diff < std::abs(autoware_universe_utils::normalizeRadian(yaw1 - yaw2)) || - !autoware_universe_utils::isDrivingForward(p1, p2)) { + max_yaw_diff < std::abs(autoware::universe_utils::normalizeRadian(yaw1 - yaw2)) || + !autoware::universe_utils::isDrivingForward(p1, p2)) { points.erase(std::next(itr)); return; } else { @@ -2068,9 +2068,9 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - autoware_universe_utils::calcSquaredDistance2d(points.at(i), pose.position); - const auto yaw = autoware_universe_utils::calcYawDeviation( - autoware_universe_utils::getPose(points.at(i)), pose); + autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose.position); + const auto yaw = autoware::universe_utils::calcYawDeviation( + autoware::universe_utils::getPose(points.at(i)), pose); if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { if (is_within_constraints) { @@ -2101,7 +2101,7 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - autoware_universe_utils::calcSquaredDistance2d(points.at(i), pose.position); + autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose.position); if (squared_dist_threshold < squared_dist) { if (is_within_constraints) { @@ -2231,13 +2231,13 @@ std::optional calcDistanceToForwardStopPoint( } const auto nearest_segment_idx = - autoware_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 = autoware_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) { @@ -2278,9 +2278,9 @@ T cropForwardPoints( } double sum_length = - -autoware_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)); + sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length) { const size_t end_idx = i; return T{points.begin(), points.begin() + end_idx}; @@ -2318,9 +2318,9 @@ T cropBackwardPoints( } double sum_length = - -autoware_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)); + sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < -backward_length) { const size_t begin_idx = i; return T{points.begin() + begin_idx, points.end()}; @@ -2419,7 +2419,7 @@ double calcYawDeviation( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + " Given points size is less than 2. Failed to calculate yaw deviation."); - autoware_universe_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -2432,12 +2432,12 @@ double calcYawDeviation( const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); - const double path_yaw = autoware_universe_utils::calcAzimuthAngle( - autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx)), - autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); + const double path_yaw = autoware::universe_utils::calcAzimuthAngle( + autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx)), + autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); const double pose_yaw = tf2::getYaw(pose.orientation); - return autoware_universe_utils::normalizeRadian(pose_yaw - path_yaw); + return autoware::universe_utils::normalizeRadian(pose_yaw - path_yaw); } extern template double calcYawDeviation>( @@ -2487,6 +2487,6 @@ extern template bool isTargetPointFront -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ diff --git a/common/autoware_motion_utils/src/distance/distance.cpp b/common/autoware_motion_utils/src/distance/distance.cpp index 244b894bad473..d31e7ec709810 100644 --- a/common/autoware_motion_utils/src/distance/distance.cpp +++ b/common/autoware_motion_utils/src/distance/distance.cpp @@ -14,7 +14,7 @@ #include "autoware/motion_utils/distance/distance.hpp" -namespace autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp index f9ae98801801f..e405cdb655c02 100644 --- a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -19,7 +19,7 @@ #include #include -namespace autoware_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(autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 5cab196e22329..388c7102b825c 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -20,10 +20,10 @@ #include -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createDeletedDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createDeletedDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; using visualization_msgs::msg::MarkerArray; namespace @@ -85,14 +85,14 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( } } // namespace -namespace autoware_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, const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = autoware_universe_utils::calcOffsetPose( + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, @@ -104,7 +104,7 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = autoware_universe_utils::calcOffsetPose( + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, @@ -116,7 +116,7 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = autoware_universe_utils::calcOffsetPose( + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, @@ -140,4 +140,4 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( { return createDeletedVirtualWallMarkerArray("dead_line_", now, id); } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 658aa66208133..4fecaea1bb838 100644 --- a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -16,7 +16,7 @@ #include "autoware/motion_utils/marker/marker_helper.hpp" -namespace autoware_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 = autoware_motion_utils::createStopVirtualWallMarker; + create_fn = autoware::motion_utils::createStopVirtualWallMarker; break; case slowdown: - create_fn = autoware_motion_utils::createSlowDownVirtualWallMarker; + create_fn = autoware::motion_utils::createSlowDownVirtualWallMarker; break; case deadline: - create_fn = autoware_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 autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index f0874264aee73..baf1c534a8a00 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -21,7 +21,7 @@ #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" -namespace autoware_motion_utils +namespace autoware::motion_utils { std::vector resamplePointVector( const std::vector & points, @@ -50,7 +50,7 @@ std::vector resamplePointVector( for (size_t i = 1; i < points.size(); ++i) { const auto & prev_pt = points.at(i - 1); const auto & curr_pt = points.at(i); - const double ds = autoware_universe_utils::calcDistance2d(prev_pt, curr_pt); + const double ds = autoware::universe_utils::calcDistance2d(prev_pt, curr_pt); input_arclength.push_back(ds + input_arclength.back()); x.push_back(curr_pt.x); y.push_back(curr_pt.y); @@ -91,7 +91,7 @@ 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 = autoware_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) { @@ -103,7 +103,7 @@ std::vector resamplePointVector( } // Insert terminal point - if (input_length - resampling_arclength.back() < autoware_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 = autoware_motion_utils::removeOverlapPoints(points_raw); + const auto points = autoware::motion_utils::removeOverlapPoints(points_raw); // validate arguments if (!resample_utils::validate_arguments(points, resampled_arclength)) { @@ -144,8 +144,8 @@ std::vector resamplePoseVector( } const bool is_driving_forward = - autoware_universe_utils::isDrivingForward(points.at(0), points.at(1)); - autoware_motion_utils::insertOrientation(resampled_points, is_driving_forward); + autoware::universe_utils::isDrivingForward(points.at(0), points.at(1)); + 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,7 +160,7 @@ 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 = autoware_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) { @@ -172,7 +172,7 @@ std::vector resamplePoseVector( } // Insert terminal point - if (input_length - resampling_arclength.back() < autoware_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 < autoware_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 < autoware_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( @@ -260,7 +260,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( const auto & prev_pt = input_path.points.at(i - 1).point; const auto & curr_pt = input_path.points.at(i).point; const double ds = - autoware_universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -370,7 +370,7 @@ 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 = autoware_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) { @@ -382,7 +382,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( } // Insert terminal point - if (input_path_len - resampling_arclength.back() < autoware_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); @@ -391,7 +391,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( // Insert stop point if (resample_input_path_stop_point) { const auto distance_to_stop_point = - autoware_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 ( @@ -401,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 < autoware_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 < autoware_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); @@ -450,7 +450,7 @@ autoware_planning_msgs::msg::Path resamplePath( const auto & prev_pt = input_path.points.at(i - 1); const auto & curr_pt = input_path.points.at(i); const double ds = - autoware_universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -512,7 +512,7 @@ autoware_planning_msgs::msg::Path resamplePath( return input_path; } - const double input_path_len = autoware_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) { @@ -524,7 +524,7 @@ autoware_planning_msgs::msg::Path resamplePath( } // Insert terminal point - if (input_path_len - resampling_arclength.back() < autoware_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); @@ -533,7 +533,7 @@ autoware_planning_msgs::msg::Path resamplePath( // Insert stop point if (resample_input_path_stop_point) { const auto distance_to_stop_point = - autoware_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 ( @@ -543,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 < autoware_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 < autoware_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); @@ -605,7 +605,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const auto & prev_pt = input_trajectory.points.at(i - 1); const auto & curr_pt = input_trajectory.points.at(i); const double ds = - autoware_universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -678,7 +678,8 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( return input_trajectory; } - const double input_trajectory_len = autoware_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) { @@ -691,7 +692,8 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( // Insert terminal point if ( - input_trajectory_len - resampling_arclength.back() < autoware_motion_utils::overlap_threshold) { + 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); @@ -700,7 +702,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( // Insert stop point if (resample_input_trajectory_stop_point) { const auto distance_to_stop_point = - autoware_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 ( @@ -710,9 +712,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 < autoware_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 < autoware_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); @@ -728,4 +730,4 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( use_zero_order_hold_for_twist); } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/conversion.cpp b/common/autoware_motion_utils/src/trajectory/conversion.cpp index 63efc7135cae2..368d3e0fbbb75 100644 --- a/common/autoware_motion_utils/src/trajectory/conversion.cpp +++ b/common/autoware_motion_utils/src/trajectory/conversion.cpp @@ -16,7 +16,7 @@ #include -namespace autoware_motion_utils +namespace autoware::motion_utils { /** * @brief Convert std::vector to @@ -51,4 +51,4 @@ std::vector convertToTrajectoryPoi return output; } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index 4b00c6344a990..4a9eaeca58d30 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -22,7 +22,7 @@ using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; -namespace autoware_motion_utils +namespace autoware::motion_utils { TrajectoryPoint calcInterpolatedPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, @@ -37,14 +37,15 @@ TrajectoryPoint calcInterpolatedPoint( return trajectory.points.front(); } - const size_t segment_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory.points, target_pose, dist_threshold, yaw_threshold); + const size_t segment_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory.points, target_pose, dist_threshold, yaw_threshold); // Calculate interpolation ratio const auto & curr_pt = trajectory.points.at(segment_idx); const auto & next_pt = trajectory.points.at(segment_idx + 1); - const auto v1 = autoware_universe_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = autoware_universe_utils::point2tfVector(curr_pt, target_pose); + const auto v1 = autoware::universe_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = autoware::universe_utils::point2tfVector(curr_pt, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -57,7 +58,7 @@ TrajectoryPoint calcInterpolatedPoint( // pose interpolation interpolated_point.pose = - autoware_universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + autoware::universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -105,14 +106,15 @@ PathPointWithLaneId calcInterpolatedPoint( return path.points.front(); } - const size_t segment_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, target_pose, dist_threshold, yaw_threshold); + const size_t segment_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, target_pose, dist_threshold, yaw_threshold); // Calculate interpolation ratio const auto & curr_pt = path.points.at(segment_idx); const auto & next_pt = path.points.at(segment_idx + 1); - const auto v1 = autoware_universe_utils::point2tfVector(curr_pt.point, next_pt.point); - const auto v2 = autoware_universe_utils::point2tfVector(curr_pt.point, target_pose); + const auto v1 = autoware::universe_utils::point2tfVector(curr_pt.point, next_pt.point); + const auto v2 = autoware::universe_utils::point2tfVector(curr_pt.point, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -125,7 +127,7 @@ PathPointWithLaneId calcInterpolatedPoint( // pose interpolation interpolated_point.point.pose = - autoware_universe_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio); + autoware::universe_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -145,4 +147,4 @@ PathPointWithLaneId calcInterpolatedPoint( return interpolated_point; } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index 52870ba2407d5..cd8de63f56c1d 100644 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -21,7 +21,7 @@ #include #include -namespace autoware_motion_utils +namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( @@ -116,15 +116,15 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( // apply beta to CoG pose geometry_msgs::msg::Pose cog_pose_with_beta; - cog_pose_with_beta.position = autoware_universe_utils::getPoint(path.points.at(i)); + cog_pose_with_beta.position = autoware::universe_utils::getPoint(path.points.at(i)); cog_pose_with_beta.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta); + autoware::universe_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta); const auto rear_pose = - autoware_universe_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); // update pose - autoware_universe_utils::setPose(rear_pose, cog_path.points.at(i)); + autoware::universe_utils::setPose(rear_pose, cog_path.points.at(i)); } // compensate for the last pose @@ -136,4 +136,4 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( return cog_path; } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index a35e6ff02b84b..5d536c0772fea 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -14,7 +14,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" -namespace autoware_motion_utils +namespace autoware::motion_utils { // @@ -599,4 +599,4 @@ template bool isTargetPointFront -namespace autoware_motion_utils +namespace autoware::motion_utils { VehicleStopCheckerBase::VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration) : clock_(node->get_clock()), logger_(node->get_logger()) @@ -118,13 +118,13 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati } const auto & p = odometry_ptr_->pose.pose.position; - const auto idx = autoware_motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); + const auto idx = autoware::motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); if (!idx) { return false; } - return std::abs(autoware_motion_utils::calcSignedArcLength( + return std::abs(autoware::motion_utils::calcSignedArcLength( trajectory_ptr_->points, p, idx.value())) < th_arrived_distance_m; } @@ -132,4 +132,4 @@ void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ptr_ = msg; } -} // namespace autoware_motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/test/src/distance/test_distance.cpp b/common/autoware_motion_utils/test/src/distance/test_distance.cpp index f32a92f5c5cbc..f6d6a9cc4dafd 100644 --- a/common/autoware_motion_utils/test/src/distance/test_distance.cpp +++ b/common/autoware_motion_utils/test/src/distance/test_distance.cpp @@ -16,7 +16,7 @@ #include "gtest/gtest.h" namespace { -using autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; constexpr double epsilon = 1e-3; diff --git a/common/autoware_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 index 5ddb0b1a1e8f8..5e2e0cc4bdf02 100644 --- a/common/autoware_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 @@ -38,9 +38,9 @@ bool has_ns_id( TEST(VirtualWallMarkerCreator, oneWall) { - autoware_motion_utils::VirtualWall wall; - autoware_motion_utils::VirtualWallMarkerCreator creator; - wall.style = autoware_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) { - autoware_motion_utils::VirtualWall wall; - autoware_motion_utils::VirtualWallMarkerCreator creator; - wall.style = autoware_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 = autoware_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 = autoware_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/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp index 1f83da0726c42..62db1b665d07a 100644 --- a/common/autoware_motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -27,13 +27,13 @@ namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromRPY; -using autoware_universe_utils::transformPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -177,7 +177,7 @@ 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 autoware::motion_utils::resamplePoseVector; using geometry_msgs::msg::Pose; std::vector path(10); @@ -220,7 +220,7 @@ TEST(resample_vector_pose, resample_by_same_interval) TEST(resample_path_with_lane_id, resample_path_by_vector) { - using autoware_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); @@ -251,7 +251,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -276,7 +276,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -540,14 +540,14 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) TEST(resample_path_with_lane_id, resample_path_by_vector_backward) { - using autoware_motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; { tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, autoware_universe_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, + i * 1.0, 0.0, 0.0, autoware::universe_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); } path.points.back().point.is_final = true; @@ -639,7 +639,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } } - const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -659,7 +659,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } path.points.back().point.is_final = true; path.points.at(0).point.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -750,7 +750,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -758,7 +758,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -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 autoware_motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Lerp x, y { @@ -919,7 +919,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -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 autoware_motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Same point resampling { @@ -1045,7 +1045,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) } // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1070,7 +1070,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(0.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -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 - autoware_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 autoware_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); @@ -1590,7 +1590,7 @@ TEST(resample_path, resample_path_by_vector) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1610,7 +1610,7 @@ TEST(resample_path, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -1812,7 +1812,7 @@ TEST(resample_path, resample_path_by_vector) TEST(resample_path, resample_path_by_vector_backward) { - using autoware_motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; { autoware_planning_msgs::msg::Path path; @@ -1884,7 +1884,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); } - const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1902,7 +1902,7 @@ TEST(resample_path, resample_path_by_vector_backward) path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); } path.points.at(0).pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -1969,7 +1969,7 @@ TEST(resample_path, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -1977,7 +1977,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1990,7 +1990,7 @@ TEST(resample_path, resample_path_by_vector_backward) TEST(resample_path, resample_path_by_vector_non_default) { - using autoware_motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Lerp x, y { @@ -2103,7 +2103,7 @@ TEST(resample_path, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -2175,7 +2175,7 @@ TEST(resample_path, resample_path_by_vector_non_default) TEST(resample_path, resample_path_by_same_interval) { - using autoware_motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Same point resampling { @@ -2205,7 +2205,7 @@ TEST(resample_path, resample_path_by_same_interval) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2225,7 +2225,7 @@ TEST(resample_path, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -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 - autoware_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 autoware_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); @@ -2660,7 +2660,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2681,7 +2681,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = traj.points.back(); - const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -2891,7 +2891,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) TEST(resample_trajectory, resample_trajectory_by_vector_non_default) { - using autoware_motion_utils::resampleTrajectory; + using autoware::motion_utils::resampleTrajectory; // Lerp x, y { @@ -3013,7 +3013,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_traj.points.size(); ++i) { const auto p = resampled_traj.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -3090,7 +3090,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) TEST(resample_trajectory, resample_trajectory_by_same_interval) { - using autoware_motion_utils::resampleTrajectory; + using autoware::motion_utils::resampleTrajectory; // Same point resampling { @@ -3122,7 +3122,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -3143,7 +3143,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = traj.points.back(); - const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -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 - autoware_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/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 6cbc0b1cc3c7e..dc828e885af64 100644 --- a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -22,9 +22,9 @@ namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; using autoware_planning_msgs::msg::Trajectory; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromRPY; constexpr double epsilon = 1e-6; @@ -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 autoware_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/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp index 8f6fb82f8066b..b4b60ff403048 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp @@ -27,11 +27,11 @@ namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromRPY; -using autoware_universe_utils::transformPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -124,7 +124,7 @@ T generateTestPath( TEST(Interpolation, interpolate_path_for_trajectory) { - using autoware_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 autoware_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 autoware_motion_utils::calcInterpolatedPose; + using autoware::motion_utils::calcInterpolatedPose; { autoware_planning_msgs::msg::Trajectory traj; diff --git a/common/autoware_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 index b9e3a59e39b29..62e4ac74cb639 100644 --- a/common/autoware_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 @@ -22,7 +22,7 @@ namespace { -using autoware_universe_utils::createPoint; +using autoware::universe_utils::createPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -31,7 +31,7 @@ geometry_msgs::msg::Pose createPose( { geometry_msgs::msg::Pose p; p.position = createPoint(x, y, z); - p.orientation = autoware_universe_utils::createQuaternionFromRPY(roll, pitch, yaw); + p.orientation = autoware::universe_utils::createQuaternionFromRPY(roll, pitch, yaw); return p; } @@ -54,7 +54,7 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { - using autoware_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 autoware_motion_utils::findNearestIndexFromLaneId; - using autoware_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 autoware_motion_utils::convertToRearWheelCenter; + using autoware::motion_utils::convertToRearWheelCenter; PathWithLaneId path; diff --git a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp index f22a2e62efa7b..4ff9b2a33ca13 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp @@ -28,9 +28,9 @@ namespace { using autoware_planning_msgs::msg::Trajectory; using TrajectoryPointArray = std::vector; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromRPY; -using autoware_universe_utils::transformPoint; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; constexpr double epsilon = 1e-6; @@ -94,7 +94,7 @@ void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) TEST(trajectory, validateNonEmpty) { - using autoware_motion_utils::validateNonEmpty; + using autoware::motion_utils::validateNonEmpty; // Empty EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); @@ -106,7 +106,7 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, validateNonSharpAngle_DefaultThreshold) { - using autoware_motion_utils::validateNonSharpAngle; + using autoware::motion_utils::validateNonSharpAngle; using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPoint p1; @@ -135,9 +135,9 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { - using autoware_motion_utils::validateNonSharpAngle; + using autoware::motion_utils::validateNonSharpAngle; + using autoware::universe_utils::pi; using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_universe_utils::pi; 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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_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 autoware_motion_utils::convertToTrajectory; + using autoware::motion_utils::convertToTrajectory; // Size check { @@ -807,7 +807,7 @@ TEST(trajectory, convertToTrajectory) TEST(trajectory, convertToTrajectoryPointArray) { - using autoware_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 autoware_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 autoware_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 autoware_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 autoware::motion_utils::calcDistanceToForwardStopPoint; + using autoware::universe_utils::deg2rad; 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 autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPoint; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::getPoint; 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 autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPoint; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::getPoint; 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 autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::getPoint; 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::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_universe_utils::deg2rad; 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::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_universe_utils::deg2rad; 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 autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::getPoint; 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::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::deg2rad; 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::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::deg2rad; 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 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; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1928,19 +1928,19 @@ 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 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; constexpr double overlap_threshold = 1e-4; auto traj = generateTestTrajectory(10, 1.0); for (size_t i = 0; i < traj.points.size(); ++i) { - traj.points.at(i).pose.orientation = createQuaternionFromYaw(autoware_universe_utils::pi); + traj.points.at(i).pose.orientation = createQuaternionFromYaw(autoware::universe_utils::pi); } const auto total_length = calcArcLength(traj.points); @@ -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 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; 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 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; 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 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; 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 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; 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 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; 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 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; 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 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; 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 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; 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 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; 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 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; 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 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; 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 autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; + using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; + using autoware::universe_utils::pi; const auto traj = generateTestTrajectory(10, 1.0); @@ -4970,7 +4970,7 @@ TEST(trajectory, findFirstNearestIndexWithSoftConstraints) TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentIndex) { - using autoware_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 autoware_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 autoware_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,101 +5246,101 @@ TEST(trajectory, removeOverlapPoints) TEST(trajectory, cropForwardPoints) { - using autoware_motion_utils::cropForwardPoints; + using autoware::motion_utils::cropForwardPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); { // Normal case const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(7)); } { // Forward length is longer than points arc length. const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } TEST(trajectory, cropBackwardPoints) { - using autoware_motion_utils::cropBackwardPoints; + using autoware::motion_utils::cropBackwardPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); { // Normal case const auto cropped_traj_points = - cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); + cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(6)); } { // Backward length is longer than points arc length. - const auto cropped_traj_points = - cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); + const auto cropped_traj_points = cropBackwardPoints( + traj.points, autoware::universe_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); + cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); + cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } TEST(trajectory, cropPoints) { - using autoware_motion_utils::cropPoints; + using autoware::motion_utils::cropPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); { // Normal case const auto cropped_traj_points = - cropPoints(traj.points, autoware_universe_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); EXPECT_EQ(cropped_traj_points.size(), static_cast(3)); } { // Length is longer than points arc length. const auto cropped_traj_points = - cropPoints(traj.points, autoware_universe_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropPoints(traj.points, autoware_universe_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropPoints(traj.points, autoware_universe_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } TEST(Trajectory, removeFirstInvalidOrientationPoints) { - using autoware_motion_utils::insertOrientation; - using autoware_motion_utils::removeFirstInvalidOrientationPoints; + using autoware::motion_utils::insertOrientation; + using autoware::motion_utils::removeFirstInvalidOrientationPoints; const double max_yaw_diff = M_PI_2; @@ -5357,7 +5357,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i)); const double yaw1 = tf2::getYaw(modified_traj.points.at(i).pose.orientation); const double yaw2 = tf2::getYaw(modified_traj.points.at(i + 1).pose.orientation); - const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian(yaw1 - yaw2)); + const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(yaw1 - yaw2)); EXPECT_LE(yaw_diff, max_yaw_diff); } }; @@ -5392,7 +5392,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) TEST(trajectory, calcYawDeviation) { - using autoware_motion_utils::calcYawDeviation; + using autoware::motion_utils::calcYawDeviation; using autoware_planning_msgs::msg::TrajectoryPoint; constexpr double tolerance = 1e-3; @@ -5419,9 +5419,9 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { - using autoware_motion_utils::isTargetPointFront; + using autoware::motion_utils::isTargetPointFront; + using autoware::universe_utils::createPoint; using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_universe_utils::createPoint; // Generate test trajectory const auto trajectory = generateTestTrajectory(10, 1.0); diff --git a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp index d18aa8d283320..b2d2f3a2e8003 100644 --- a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp +++ b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp @@ -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 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 nav_msgs::msg::Odometry; class CheckerNode : public rclcpp::Node diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index b4b5834f71fa1..49d2c848ab508 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -62,8 +62,8 @@ using autoware_planning_msgs::msg::Trajectory; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp index c90cc714367f1..a0e24c68b7918 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp @@ -25,7 +25,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { // 2D struct Point2d; @@ -93,12 +93,12 @@ inline Point3d fromMsg(const geometry_msgs::msg::Point & msg) point.z() = msg.z; return point; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils -BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT - autoware_universe_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT -BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT - autoware_universe_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT -BOOST_GEOMETRY_REGISTER_RING(autoware_universe_utils::LinearRing2d) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT + autoware::universe_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT + autoware::universe_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT +BOOST_GEOMETRY_REGISTER_RING(autoware::universe_utils::LinearRing2d) // NOLINT #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp index 477a1b4c27a6c..1313ec558fe01 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp @@ -24,7 +24,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { bool isClockwise(const Polygon2d & polygon); Polygon2d inverseClockwise(const Polygon2d & polygon); @@ -47,6 +47,6 @@ Polygon2d toFootprint( const double base_to_rear, const double width); double getArea(const autoware_perception_msgs::msg::Shape & shape); Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp index 0af9d30ad09c1..87c06dfd9cf08 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp @@ -96,7 +96,7 @@ inline void doTransform( #endif } // namespace tf2 -namespace autoware_universe_utils +namespace autoware::universe_utils { template geometry_msgs::msg::Point getPoint(const T & p) @@ -577,6 +577,6 @@ std::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp index 2f2b4a5d3dfb0..c3a602caf4232 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp @@ -18,7 +18,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { struct PoseDeviation { @@ -39,6 +39,6 @@ double calcYawDeviation( PoseDeviation calcPoseDeviation( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp index 2c1f9fe646723..f06770c2925c7 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ #define AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ -namespace autoware_universe_utils +namespace autoware::universe_utils { constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20 constexpr double gravity = 9.80665; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp index 502ed4b4f7b34..16af7f44415cc 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp @@ -19,7 +19,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { inline double normalizeDegree(const double deg, const double min_deg = -180) { @@ -45,6 +45,6 @@ inline double normalizeRadian(const double rad, const double min_rad = -pi) return value - std::copysign(2 * pi, value); } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__MATH__NORMALIZATION_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp index fabf67a683680..ed0ef455bfcfe 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp @@ -20,7 +20,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template std::vector arange(const T start, const T stop, const T step = 1) @@ -73,6 +73,6 @@ std::vector linspace(const T start, const T stop, const size_t num) return out; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__MATH__RANGE_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp index bf67a1b521b6d..e87d8a2e5fc40 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp @@ -17,7 +17,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { constexpr size_t sin_table_size = 32769; @@ -25,6 +25,6 @@ constexpr size_t discrete_arcs_num_90 = 32768; constexpr size_t discrete_arcs_num_360 = 131072; extern const float g_sin_table[sin_table_size]; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__MATH__SIN_TABLE_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp index 7f58735d35963..19a59523c7f08 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp @@ -17,7 +17,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { float sin(float radian); @@ -26,6 +26,6 @@ float cos(float radian); std::pair sin_and_cos(float radian); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp index 66be4d99d5803..36ce8e9f39514 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp @@ -17,7 +17,7 @@ #include "autoware/universe_utils/math/constants.hpp" -namespace autoware_universe_utils +namespace autoware::universe_utils { constexpr double deg2rad(const double deg) { @@ -36,6 +36,6 @@ constexpr double mps2kmph(const double mps) { return mps * 3600.0 / 1000.0; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp index 0965f8ffbaefa..acd82995b1d84 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp @@ -25,14 +25,14 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { namespace debug_publisher { template < class T_msg, class T, std::enable_if_t< - autoware_universe_utils::debug_traits::is_debug_message::value, std::nullptr_t> = + autoware::universe_utils::debug_traits::is_debug_message::value, std::nullptr_t> = nullptr> T_msg toDebugMsg(const T & data, const rclcpp::Time & stamp) { @@ -73,6 +73,6 @@ class DebugPublisher const char * ns_; std::unordered_map> pub_map_; }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp index 330b01b68a0b2..8420f930e0ce9 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp @@ -28,7 +28,7 @@ #include -namespace autoware_universe_utils::debug_traits +namespace autoware::universe_utils::debug_traits { template struct is_debug_message : std::false_type @@ -84,6 +84,6 @@ template <> struct is_debug_message : std::true_type { }; -} // namespace autoware_universe_utils::debug_traits +} // namespace autoware::universe_utils::debug_traits #endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp index 41163ea38b018..b1b11d33ab448 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp @@ -27,7 +27,7 @@ // ... // // // Define logger_configure as a node class member variable -// std::unique_ptr logger_configure_; +// std::unique_ptr logger_configure_; // } // // ___In your_node.cpp___ @@ -45,7 +45,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { class LoggerLevelConfigure { @@ -64,5 +64,5 @@ class LoggerLevelConfigure const ConfigLogger::Response::SharedPtr response); }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp index ad2d2f84f82e0..6991962c420d1 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp @@ -22,7 +22,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) { @@ -76,6 +76,6 @@ void appendMarkerArray( visualization_msgs::msg::MarkerArray * marker_array, const std::optional & current_time = {}); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp index 55caf1955b6ec..ff957a878440e 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ #define AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ -namespace autoware_universe_utils +namespace autoware::universe_utils { namespace xyz_covariance_index { @@ -115,6 +115,6 @@ enum XYZ_UPPER_COV_IDX { Z_Z = 5, }; } // namespace xyz_upper_covariance_index -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp index 5ef1837e0cece..89846688d365d 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp @@ -19,7 +19,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) @@ -30,6 +30,6 @@ T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) return node.declare_parameter(name); } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__PARAMETER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp index cce8a75b37aa9..7fab38790921a 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp @@ -20,7 +20,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { /** * @brief a faster implementation of converting sensor_msgs::msg::PointCloud2 to @@ -67,6 +67,6 @@ void transformPointCloudFromROSMsg( } } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__PCL_CONVERSION_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp index a41e13229a826..1b7ea0bd69951 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { inline rclcpp::SensorDataQoS SingleDepthSensorQoS() @@ -159,6 +159,6 @@ class InterProcessPollingSubscriber= 2)>::typ }; }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp index bb92d615b0a8d..96cddc595a5b9 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { class ProcessingTimePublisher { @@ -62,6 +62,6 @@ class ProcessingTimePublisher return oss.str(); } }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp index 960295944a14b..60112d9e83cf3 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp @@ -26,7 +26,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { class PublishedTimePublisher @@ -109,6 +109,6 @@ class PublishedTimePublisher // store them for each different publisher of the node std::map::SharedPtr, GidCompare> publishers_; }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp index 0bd9a4db5b674..cdcf78a9c5edc 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp @@ -22,7 +22,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { class SelfPoseListener { @@ -53,6 +53,6 @@ class SelfPoseListener private: TransformListener transform_listener_; }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp index 176a11dcde529..e053ef3668034 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp @@ -26,7 +26,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { class TransformListener { @@ -82,6 +82,6 @@ class TransformListener std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp index 8bb505d765aef..bebb38af0b261 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp @@ -20,7 +20,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template bool updateParam(const std::vector & params, const std::string & name, T & value) @@ -37,6 +37,6 @@ bool updateParam(const std::vector & params, const std::strin value = itr->template get_value(); return true; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__UPDATE_PARAM_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp index 7d4892f724848..62c9f75f3f233 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { inline unique_identifier_msgs::msg::UUID generateUUID() { @@ -58,6 +58,6 @@ inline unique_identifier_msgs::msg::UUID toUUIDMsg(const boost::uuids::uuid & id return ros_uuid; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__UUID_HELPER_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp index 0aaef92835653..1faaaa861b1c4 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp @@ -21,7 +21,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template T waitForParam( @@ -47,6 +47,6 @@ T waitForParam( return {}; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp index f195cd2b32391..0b025160bacf8 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp @@ -15,11 +15,11 @@ #ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ #define AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ -namespace autoware_universe_utils +namespace autoware::universe_utils { void print_backtrace(); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp index 3df651825f1aa..eb5cb546b8c7f 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp @@ -19,7 +19,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template < class OutputUnit = std::chrono::seconds, class InternalUnit = std::chrono::microseconds, @@ -58,6 +58,6 @@ class StopWatch std::unordered_map t_start_; }; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__STOP_WATCH_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp index f480a709d169e..8a92116b426ec 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp @@ -21,7 +21,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template void transformPointCloud( @@ -46,6 +46,6 @@ void transformPointCloud( pcl::transformPointCloud(cloud_in, cloud_out, transform); } } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__TRANSFORM__TRANSFORMS_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp b/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp index ce9232a8c1add..da3da42cbc41b 100644 --- a/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp @@ -23,8 +23,8 @@ namespace { namespace bg = boost::geometry; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -68,7 +68,7 @@ double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) } } // namespace -namespace autoware_universe_utils +namespace autoware::universe_utils { bool isClockwise(const Polygon2d & polygon) { @@ -123,16 +123,16 @@ Polygon2d toPolygon2d( Polygon2d polygon; if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - const auto point0 = autoware_universe_utils::calcOffsetPose( + const auto point0 = autoware::universe_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; - const auto point1 = autoware_universe_utils::calcOffsetPose( + const auto point1 = autoware::universe_utils::calcOffsetPose( pose, -shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; - const auto point2 = autoware_universe_utils::calcOffsetPose( + const auto point2 = autoware::universe_utils::calcOffsetPose( pose, -shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) .position; - const auto point3 = autoware_universe_utils::calcOffsetPose( + const auto point3 = autoware::universe_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) .position; @@ -179,24 +179,24 @@ Polygon2d toPolygon2d( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } -autoware_universe_utils::Polygon2d toPolygon2d( +autoware::universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::DetectedObject & object) { - return autoware_universe_utils::toPolygon2d( + return autoware::universe_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } -autoware_universe_utils::Polygon2d toPolygon2d( +autoware::universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::TrackedObject & object) { - return autoware_universe_utils::toPolygon2d( + return autoware::universe_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } -autoware_universe_utils::Polygon2d toPolygon2d( +autoware::universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::PredictedObject & object) { - return autoware_universe_utils::toPolygon2d( + return autoware::universe_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); } @@ -206,16 +206,16 @@ Polygon2d toFootprint( { Polygon2d polygon; const auto point0 = - autoware_universe_utils::calcOffsetPose(base_link_pose, base_to_front, width / 2.0, 0.0) + autoware::universe_utils::calcOffsetPose(base_link_pose, base_to_front, width / 2.0, 0.0) .position; const auto point1 = - autoware_universe_utils::calcOffsetPose(base_link_pose, base_to_front, -width / 2.0, 0.0) + autoware::universe_utils::calcOffsetPose(base_link_pose, base_to_front, -width / 2.0, 0.0) .position; const auto point2 = - autoware_universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, -width / 2.0, 0.0) + autoware::universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, -width / 2.0, 0.0) .position; const auto point3 = - autoware_universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, width / 2.0, 0.0) + autoware::universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, width / 2.0, 0.0) .position; appendPointToPolygon(polygon, point0); @@ -272,4 +272,4 @@ Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset) boost::geometry::correct(expanded_polygon); return expanded_polygon; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/geometry/geometry.cpp b/common/autoware_universe_utils/src/geometry/geometry.cpp index e6a491671595a..1f4fd318e227b 100644 --- a/common/autoware_universe_utils/src/geometry/geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/geometry.cpp @@ -32,7 +32,7 @@ void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped().x(point32.x).y(point32.y).z(point32.z); - const auto transformed_point = autoware_universe_utils::transformPoint(point, pose); + const auto transformed_point = autoware::universe_utils::transformPoint(point, pose); return geometry_msgs::build() .x(transformed_point.x) .y(transformed_point.y) @@ -383,4 +383,4 @@ std::optional intersect( return intersect_point; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/geometry/pose_deviation.cpp b/common/autoware_universe_utils/src/geometry/pose_deviation.cpp index 5c81519510c8e..77849e73f5dfd 100644 --- a/common/autoware_universe_utils/src/geometry/pose_deviation.cpp +++ b/common/autoware_universe_utils/src/geometry/pose_deviation.cpp @@ -28,7 +28,7 @@ #include #endif -namespace autoware_universe_utils +namespace autoware::universe_utils { double calcLateralDeviation( @@ -82,4 +82,4 @@ PoseDeviation calcPoseDeviation( return deviation; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/math/sin_table.cpp b/common/autoware_universe_utils/src/math/sin_table.cpp index bc0a5d7b09024..ce021869176ee 100644 --- a/common/autoware_universe_utils/src/math/sin_table.cpp +++ b/common/autoware_universe_utils/src/math/sin_table.cpp @@ -14,7 +14,7 @@ #include "autoware/universe_utils/math/sin_table.hpp" -namespace autoware_universe_utils +namespace autoware::universe_utils { const float g_sin_table[sin_table_size] = { @@ -8212,4 +8212,4 @@ const float g_sin_table[sin_table_size] = { 0.9999999816164293f, 0.9999999896592414f, 0.9999999954041073f, 0.9999999988510269f, 1.0000000000000000f}; -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp index 40a993343b1de..586b9075ba6d5 100644 --- a/common/autoware_universe_utils/src/math/trigonometry.cpp +++ b/common/autoware_universe_utils/src/math/trigonometry.cpp @@ -19,12 +19,12 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { float sin(float radian) { - float degree = radian * (180.f / static_cast(autoware_universe_utils::pi)) * + float degree = radian * (180.f / static_cast(autoware::universe_utils::pi)) * (discrete_arcs_num_360 / 360.f); size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % @@ -46,13 +46,13 @@ float sin(float radian) float cos(float radian) { - return sin(radian + static_cast(autoware_universe_utils::pi) / 2.f); + return sin(radian + static_cast(autoware::universe_utils::pi) / 2.f); } std::pair sin_and_cos(float radian) { constexpr float tmp = - (180.f / static_cast(autoware_universe_utils::pi)) * (discrete_arcs_num_360 / 360.f); + (180.f / static_cast(autoware::universe_utils::pi)) * (discrete_arcs_num_360 / 360.f); const float degree = radian * tmp; size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % @@ -72,4 +72,4 @@ std::pair sin_and_cos(float radian) } } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/ros/logger_level_configure.cpp b/common/autoware_universe_utils/src/ros/logger_level_configure.cpp index 77e8d9bf16d7f..d517b81b93224 100644 --- a/common/autoware_universe_utils/src/ros/logger_level_configure.cpp +++ b/common/autoware_universe_utils/src/ros/logger_level_configure.cpp @@ -16,7 +16,7 @@ #include -namespace autoware_universe_utils +namespace autoware::universe_utils { LoggerLevelConfigure::LoggerLevelConfigure(rclcpp::Node * node) : ros_logger_(node->get_logger()) { @@ -58,4 +58,4 @@ void LoggerLevelConfigure::onLoggerConfigService( return; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp index d4f97af2ef90f..507be41dea7bb 100644 --- a/common/autoware_universe_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -14,7 +14,7 @@ #include "autoware/universe_utils/ros/marker_helper.hpp" -namespace autoware_universe_utils +namespace autoware::universe_utils { visualization_msgs::msg::Marker createDefaultMarker( @@ -68,4 +68,4 @@ void appendMarkerArray( } } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/system/backtrace.cpp b/common/autoware_universe_utils/src/system/backtrace.cpp index f4c4312ce44fb..7af91bdcc60dc 100644 --- a/common/autoware_universe_utils/src/system/backtrace.cpp +++ b/common/autoware_universe_utils/src/system/backtrace.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { void print_backtrace() @@ -49,4 +49,4 @@ void print_backtrace() free(symbol_list); } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp index 764bb388fc74a..7c688e1982838 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp @@ -20,8 +20,8 @@ namespace bg = boost::geometry; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Point3d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; TEST(boost_geometry, boost_geometry_distance) { @@ -54,7 +54,7 @@ TEST(boost_geometry, to_2d) TEST(boost_geometry, toMsg) { - using autoware_universe_utils::toMsg; + using autoware::universe_utils::toMsg; { const Point3d p(1.0, 2.0, 3.0); @@ -68,7 +68,7 @@ TEST(boost_geometry, toMsg) TEST(boost_geometry, fromMsg) { - using autoware_universe_utils::fromMsg; + using autoware::universe_utils::fromMsg; geometry_msgs::msg::Point p_msg; p_msg.x = 1.0; diff --git a/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp index ed576ba0dceaf..4b75e8130a253 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -19,7 +19,7 @@ #include -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::Polygon2d; namespace { @@ -39,7 +39,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double p.position.x = x; p.position.y = y; p.position.z = 0.0; - p.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + p.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return p; } @@ -47,7 +47,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(boost_geometry, boost_isClockwise) { - using autoware_universe_utils::isClockwise; + using autoware::universe_utils::isClockwise; // empty Polygon2d empty_polygon; @@ -72,8 +72,8 @@ TEST(boost_geometry, boost_isClockwise) TEST(boost_geometry, boost_inverseClockwise) { - using autoware_universe_utils::inverseClockwise; - using autoware_universe_utils::isClockwise; + using autoware::universe_utils::inverseClockwise; + using autoware::universe_utils::isClockwise; // empty Polygon2d empty_polygon; @@ -100,7 +100,7 @@ TEST(boost_geometry, boost_inverseClockwise) TEST(boost_geometry, boost_rotatePolygon) { constexpr double epsilon = 1e-6; - using autoware_universe_utils::rotatePolygon; + using autoware::universe_utils::rotatePolygon; // empty geometry_msgs::msg::Polygon empty_polygon; @@ -130,7 +130,7 @@ TEST(boost_geometry, boost_rotatePolygon) TEST(boost_geometry, boost_toPolygon2d) { - using autoware_universe_utils::toPolygon2d; + using autoware::universe_utils::toPolygon2d; { // bounding box const double x = 1.0; @@ -206,7 +206,7 @@ TEST(boost_geometry, boost_toPolygon2d) TEST(boost_geometry, boost_toFootprint) { - using autoware_universe_utils::toFootprint; + using autoware::universe_utils::toFootprint; // from base link { @@ -234,7 +234,7 @@ TEST(boost_geometry, boost_toFootprint) TEST(boost_geometry, boost_getArea) { - using autoware_universe_utils::getArea; + using autoware::universe_utils::getArea; { // bounding box const double x = 1.0; @@ -290,7 +290,7 @@ TEST(boost_geometry, boost_getArea) TEST(boost_geometry, boost_expandPolygon) { - using autoware_universe_utils::expandPolygon; + using autoware::universe_utils::expandPolygon; { // box with a certain offset Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}}; diff --git a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index 37e43c0ce08dd..194cd7d503d12 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-6; TEST(geometry, getPoint) { - using autoware_universe_utils::getPoint; + using autoware::universe_utils::getPoint; const double x_ans = 1.0; const double y_ans = 2.0; @@ -114,7 +114,7 @@ TEST(geometry, getPoint) TEST(geometry, getPose) { - using autoware_universe_utils::getPose; + using autoware::universe_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -203,7 +203,7 @@ TEST(geometry, getPose) TEST(geometry, getLongitudinalVelocity) { - using autoware_universe_utils::getLongitudinalVelocity; + using autoware::universe_utils::getLongitudinalVelocity; const double velocity = 1.0; @@ -222,7 +222,7 @@ TEST(geometry, getLongitudinalVelocity) TEST(geometry, setPose) { - using autoware_universe_utils::setPose; + using autoware::universe_utils::setPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -292,9 +292,9 @@ TEST(geometry, setPose) TEST(geometry, setOrientation) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::setOrientation; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::setOrientation; geometry_msgs::msg::Pose p; const auto orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); @@ -308,7 +308,7 @@ TEST(geometry, setOrientation) TEST(geometry, setLongitudinalVelocity) { - using autoware_universe_utils::setLongitudinalVelocity; + using autoware::universe_utils::setLongitudinalVelocity; const double velocity = 1.0; @@ -327,7 +327,7 @@ TEST(geometry, setLongitudinalVelocity) TEST(geometry, createPoint) { - using autoware_universe_utils::createPoint; + using autoware::universe_utils::createPoint; const geometry_msgs::msg::Point p_out = createPoint(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(p_out.x, 1.0); @@ -337,7 +337,7 @@ TEST(geometry, createPoint) TEST(geometry, createQuaternion) { - using autoware_universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternion; // (0.18257419, 0.36514837, 0.54772256, 0.73029674) is normalized quaternion of (1, 2, 3, 4) const geometry_msgs::msg::Quaternion q_out = @@ -350,7 +350,7 @@ TEST(geometry, createQuaternion) TEST(geometry, createTranslation) { - using autoware_universe_utils::createTranslation; + using autoware::universe_utils::createTranslation; const geometry_msgs::msg::Vector3 v_out = createTranslation(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(v_out.x, 1.0); @@ -360,8 +360,8 @@ TEST(geometry, createTranslation) TEST(geometry, createQuaternionFromRPY) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; { const auto q_out = createQuaternionFromRPY(0, 0, 0); @@ -390,8 +390,8 @@ TEST(geometry, createQuaternionFromRPY) TEST(geometry, createQuaternionFromYaw) { - using autoware_universe_utils::createQuaternionFromYaw; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; { const auto q_out = createQuaternionFromYaw(0); @@ -420,9 +420,9 @@ TEST(geometry, createQuaternionFromYaw) TEST(geometry, calcElevationAngle) { - using autoware_universe_utils::calcElevationAngle; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::calcElevationAngle; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; { const auto p1 = createPoint(1.0, 1.0, 1.0); @@ -468,9 +468,9 @@ TEST(geometry, calcElevationAngle) TEST(geometry, calcAzimuthAngle) { - using autoware_universe_utils::calcAzimuthAngle; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; { const auto p1 = createPoint(0.0, 0.0, 9.0); @@ -521,7 +521,7 @@ TEST(geometry, calcAzimuthAngle) TEST(geometry, calcDistance2d) { - using autoware_universe_utils::calcDistance2d; + using autoware::universe_utils::calcDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -538,7 +538,7 @@ TEST(geometry, calcDistance2d) TEST(geometry, calcSquaredDistance2d) { - using autoware_universe_utils::calcSquaredDistance2d; + using autoware::universe_utils::calcSquaredDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -555,7 +555,7 @@ TEST(geometry, calcSquaredDistance2d) TEST(geometry, calcDistance3d) { - using autoware_universe_utils::calcDistance3d; + using autoware::universe_utils::calcDistance3d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -572,9 +572,9 @@ TEST(geometry, calcDistance3d) TEST(geometry, getRPY) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::getRPY; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getRPY; { const double ans_roll = deg2rad(5); @@ -610,9 +610,9 @@ TEST(geometry, getRPY) TEST(geometry, getRPY_wrapper) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::getRPY; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getRPY; { const double ans_roll = deg2rad(45); @@ -652,9 +652,9 @@ TEST(geometry, getRPY_wrapper) TEST(geometry, transform2pose) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::transform2pose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::transform2pose; { geometry_msgs::msg::Transform transform; @@ -703,9 +703,9 @@ TEST(geometry, transform2pose) TEST(geometry, pose2transform) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::pose2transform; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::pose2transform; { geometry_msgs::msg::Pose pose; @@ -756,9 +756,9 @@ TEST(geometry, pose2transform) TEST(geometry, point2tfVector) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::point2tfVector; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::point2tfVector; // Point { @@ -823,11 +823,11 @@ TEST(geometry, point2tfVector) TEST(geometry, transformPoint) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::Point2d; - using autoware_universe_utils::Point3d; - using autoware_universe_utils::transformPoint; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::Point2d; + using autoware::universe_utils::Point3d; + using autoware::universe_utils::transformPoint; { const Point2d p(1.0, 2.0); @@ -916,9 +916,9 @@ TEST(geometry, transformPoint) TEST(geometry, transformPose) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::transformPose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::transformPose; geometry_msgs::msg::Pose pose; pose.position.x = 2.0; @@ -967,9 +967,9 @@ TEST(geometry, transformPose) TEST(geometry, inverseTransformPose) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::inverseTransformPose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::inverseTransformPose; geometry_msgs::msg::Pose pose; pose.position.x = 2.0; @@ -1018,10 +1018,10 @@ TEST(geometry, inverseTransformPose) TEST(geometry, inverseTransformPoint) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::inverseTransformPoint; - using autoware_universe_utils::inverseTransformPose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::inverseTransformPoint; + using autoware::universe_utils::inverseTransformPose; geometry_msgs::msg::Pose pose_transform; pose_transform.position.x = 1.0; @@ -1050,10 +1050,10 @@ TEST(geometry, inverseTransformPoint) TEST(geometry, transformVector) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::MultiPoint3d; - using autoware_universe_utils::transformVector; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::MultiPoint3d; + using autoware::universe_utils::transformVector; { const MultiPoint3d ps{{1.0, 2.0, 3.0}, {2.0, 3.0, 4.0}}; @@ -1078,51 +1078,51 @@ TEST(geometry, transformVector) TEST(geometry, calcCurvature) { - using autoware_universe_utils::calcCurvature; + using autoware::universe_utils::calcCurvature; // Straight Line { - geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.0); } // Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -1.0); } // Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -0.2); } // Counter-Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(-1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(-2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(-1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(-2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 1.0); } // Counter-Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(-5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(-10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(-5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(-10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.2); } // Give same points { - geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); ASSERT_ANY_THROW(calcCurvature(p1, p1, p1)); ASSERT_ANY_THROW(calcCurvature(p1, p1, p2)); ASSERT_ANY_THROW(calcCurvature(p1, p2, p1)); @@ -1132,11 +1132,11 @@ TEST(geometry, calcCurvature) TEST(geometry, calcOffsetPose) { - using autoware_universe_utils::calcOffsetPose; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::createQuaternion; - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::calcOffsetPose; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; // Only translation { @@ -1224,12 +1224,12 @@ TEST(geometry, calcOffsetPose) TEST(geometry, isDrivingForward) { - using autoware_universe_utils::calcInterpolatedPoint; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::createQuaternion; - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; - using autoware_universe_utils::isDrivingForward; + using autoware::universe_utils::calcInterpolatedPoint; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::isDrivingForward; const double epsilon = 1e-3; @@ -1286,8 +1286,8 @@ TEST(geometry, isDrivingForward) TEST(geometry, calcInterpolatedPoint) { - using autoware_universe_utils::calcInterpolatedPoint; - using autoware_universe_utils::createPoint; + using autoware::universe_utils::calcInterpolatedPoint; + using autoware::universe_utils::createPoint; { const auto src_point = createPoint(0.0, 0.0, 0.0); @@ -1343,11 +1343,11 @@ TEST(geometry, calcInterpolatedPoint) TEST(geometry, calcInterpolatedPose) { - using autoware_universe_utils::calcInterpolatedPose; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::createQuaternion; - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::calcInterpolatedPose; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; const double epsilon = 1e-3; // Position Interpolation @@ -1527,11 +1527,11 @@ TEST(geometry, calcInterpolatedPose) TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) { - using autoware_universe_utils::calcInterpolatedPose; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::createQuaternion; - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::calcInterpolatedPose; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; const double epsilon = 1e-3; // Position Interpolation @@ -1675,7 +1675,7 @@ TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) TEST(geometry, getTwist) { - using autoware_universe_utils::createVector3; + using autoware::universe_utils::createVector3; geometry_msgs::msg::Vector3 velocity = createVector3(1.0, 2.0, 3.0); geometry_msgs::msg::Vector3 angular = createVector3(0.1, 0.2, 0.3); @@ -1691,7 +1691,7 @@ TEST(geometry, getTwist) // getTwist { - auto t_out = autoware_universe_utils::createTwist(velocity, angular); + auto t_out = autoware::universe_utils::createTwist(velocity, angular); EXPECT_DOUBLE_EQ(t_out.linear.x, twist.linear.x); EXPECT_DOUBLE_EQ(t_out.linear.y, twist.linear.y); EXPECT_DOUBLE_EQ(t_out.linear.z, twist.linear.z); @@ -1703,32 +1703,32 @@ TEST(geometry, getTwist) TEST(geometry, getTwistNorm) { - using autoware_universe_utils::createVector3; + using autoware::universe_utils::createVector3; geometry_msgs::msg::TwistWithCovariance twist_with_covariance; twist_with_covariance.twist = geometry_msgs::build() .linear(createVector3(3.0, 4.0, 0.0)) .angular(createVector3(0.1, 0.2, 0.3)); - EXPECT_NEAR(autoware_universe_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); + EXPECT_NEAR(autoware::universe_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); } TEST(geometry, isTwistCovarianceValid) { - using autoware_universe_utils::createVector3; + using autoware::universe_utils::createVector3; geometry_msgs::msg::TwistWithCovariance twist_with_covariance; twist_with_covariance.twist = geometry_msgs::build() .linear(createVector3(1.0, 2.0, 3.0)) .angular(createVector3(0.1, 0.2, 0.3)); - EXPECT_EQ(autoware_universe_utils::isTwistCovarianceValid(twist_with_covariance), false); + EXPECT_EQ(autoware::universe_utils::isTwistCovarianceValid(twist_with_covariance), false); twist_with_covariance.covariance.at(0) = 1.0; - EXPECT_EQ(autoware_universe_utils::isTwistCovarianceValid(twist_with_covariance), true); + EXPECT_EQ(autoware::universe_utils::isTwistCovarianceValid(twist_with_covariance), true); } TEST(geometry, intersect) { - using autoware_universe_utils::createPoint; - using autoware_universe_utils::intersect; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::intersect; { // Normally crossing const auto p1 = createPoint(0.0, -1.0, 0.0); diff --git a/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index 058149af2c192..4ed1c5497c6ae 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -18,7 +18,7 @@ TEST(geometry, getPoint_PathWithLaneId) { - using autoware_universe_utils::getPoint; + using autoware::universe_utils::getPoint; const double x_ans = 1.0; const double y_ans = 2.0; @@ -36,7 +36,7 @@ TEST(geometry, getPoint_PathWithLaneId) TEST(geometry, getPose_PathWithLaneId) { - using autoware_universe_utils::getPose; + using autoware::universe_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -66,7 +66,7 @@ TEST(geometry, getPose_PathWithLaneId) TEST(geometry, getLongitudinalVelocity_PathWithLaneId) { - using autoware_universe_utils::getLongitudinalVelocity; + using autoware::universe_utils::getLongitudinalVelocity; const double velocity = 1.0; @@ -77,7 +77,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) TEST(geometry, setPose_PathWithLaneId) { - using autoware_universe_utils::setPose; + using autoware::universe_utils::setPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -109,7 +109,7 @@ TEST(geometry, setPose_PathWithLaneId) TEST(geometry, setLongitudinalVelocity_PathWithLaneId) { - using autoware_universe_utils::setLongitudinalVelocity; + using autoware::universe_utils::setLongitudinalVelocity; const double velocity = 1.0; diff --git a/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp b/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp index 0808223bb9ea2..c9a5c2cec4fac 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp @@ -20,9 +20,9 @@ TEST(geometry, pose_deviation) { - using autoware_universe_utils::calcPoseDeviation; - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::calcPoseDeviation; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; geometry_msgs::msg::Pose pose1; pose1.position.x = 1.0; diff --git a/common/autoware_universe_utils/test/src/math/test_constants.cpp b/common/autoware_universe_utils/test/src/math/test_constants.cpp index 47e76efd66f6e..6c449834157b0 100644 --- a/common/autoware_universe_utils/test/src/math/test_constants.cpp +++ b/common/autoware_universe_utils/test/src/math/test_constants.cpp @@ -18,14 +18,14 @@ TEST(constants, pi) { - using autoware_universe_utils::pi; + using autoware::universe_utils::pi; EXPECT_DOUBLE_EQ(pi, 3.14159265358979323846); } TEST(constants, gravity) { - using autoware_universe_utils::gravity; + using autoware::universe_utils::gravity; EXPECT_DOUBLE_EQ(gravity, 9.80665); } diff --git a/common/autoware_universe_utils/test/src/math/test_normalization.cpp b/common/autoware_universe_utils/test/src/math/test_normalization.cpp index 19b8a29019057..ae7f3cd7a7a4c 100644 --- a/common/autoware_universe_utils/test/src/math/test_normalization.cpp +++ b/common/autoware_universe_utils/test/src/math/test_normalization.cpp @@ -18,7 +18,7 @@ TEST(normalization, normalizeDegree) { - using autoware_universe_utils::normalizeDegree; + using autoware::universe_utils::normalizeDegree; // -180 <= deg < 180 { @@ -51,7 +51,7 @@ TEST(normalization, normalizeDegree) TEST(normalization, normalizeRadian) { - using autoware_universe_utils::normalizeRadian; + using autoware::universe_utils::normalizeRadian; // -M_PI <= deg < M_PI { diff --git a/common/autoware_universe_utils/test/src/math/test_range.cpp b/common/autoware_universe_utils/test/src/math/test_range.cpp index 400a6a4dd20c1..12be5b646957b 100644 --- a/common/autoware_universe_utils/test/src/math/test_range.cpp +++ b/common/autoware_universe_utils/test/src/math/test_range.cpp @@ -44,7 +44,7 @@ void expect_eq_vector(const std::vector & input, const std::vector & e TEST(arange_Test, arange_double) { - using autoware_universe_utils::arange; + using autoware::universe_utils::arange; // general cases { @@ -82,7 +82,7 @@ TEST(arange_Test, arange_double) TEST(arange_Test, arange_float) { - using autoware_universe_utils::arange; + using autoware::universe_utils::arange; // general cases { @@ -121,7 +121,7 @@ TEST(arange_Test, arange_float) TEST(arange_Test, arange_int) { - using autoware_universe_utils::arange; + using autoware::universe_utils::arange; // general cases { @@ -154,7 +154,7 @@ TEST(arange_Test, arange_int) TEST(test_linspace, linspace_double) { - using autoware_universe_utils::linspace; + using autoware::universe_utils::linspace; // general cases { @@ -182,7 +182,7 @@ TEST(test_linspace, linspace_double) TEST(test_linspace, linspace_float) { - using autoware_universe_utils::linspace; + using autoware::universe_utils::linspace; // general cases { @@ -211,7 +211,7 @@ TEST(test_linspace, linspace_float) TEST(test_linspace, linspace_int) { - using autoware_universe_utils::linspace; + using autoware::universe_utils::linspace; // general cases { diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp index c4ba81a6b77ff..b55b27a34a6ac 100644 --- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp +++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp @@ -21,31 +21,31 @@ TEST(trigonometry, sin) { - float x = 4.f * autoware_universe_utils::pi / 128.f; + float x = 4.f * autoware::universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { EXPECT_TRUE( std::abs( std::sin(x * static_cast(i)) - - autoware_universe_utils::sin(x * static_cast(i))) < 10e-5); + autoware::universe_utils::sin(x * static_cast(i))) < 10e-5); } } TEST(trigonometry, cos) { - float x = 4.f * autoware_universe_utils::pi / 128.f; + float x = 4.f * autoware::universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { EXPECT_TRUE( std::abs( std::cos(x * static_cast(i)) - - autoware_universe_utils::cos(x * static_cast(i))) < 10e-5); + autoware::universe_utils::cos(x * static_cast(i))) < 10e-5); } } TEST(trigonometry, sin_and_cos) { - float x = 4.f * autoware_universe_utils::pi / 128.f; + float x = 4.f * autoware::universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { - const auto sin_and_cos = autoware_universe_utils::sin_and_cos(x * static_cast(i)); + const auto sin_and_cos = autoware::universe_utils::sin_and_cos(x * static_cast(i)); EXPECT_TRUE(std::abs(std::sin(x * static_cast(i)) - sin_and_cos.first) < 10e-7); EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); } diff --git a/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp b/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp index e00b85f9e24f2..605b0e7d3c5a6 100644 --- a/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp +++ b/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp @@ -16,11 +16,11 @@ #include -using autoware_universe_utils::pi; +using autoware::universe_utils::pi; TEST(unit_conversion, deg2rad) { - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::deg2rad; EXPECT_DOUBLE_EQ(deg2rad(-720), -4 * pi); EXPECT_DOUBLE_EQ(deg2rad(0), 0); @@ -33,7 +33,7 @@ TEST(unit_conversion, deg2rad) TEST(unit_conversion, rad2deg) { - using autoware_universe_utils::rad2deg; + using autoware::universe_utils::rad2deg; EXPECT_DOUBLE_EQ(rad2deg(-4 * pi), -720); EXPECT_DOUBLE_EQ(rad2deg(0), 0); @@ -46,7 +46,7 @@ TEST(unit_conversion, rad2deg) TEST(unit_conversion, kmph2mps) { - using autoware_universe_utils::kmph2mps; + using autoware::universe_utils::kmph2mps; EXPECT_DOUBLE_EQ(kmph2mps(0), 0); EXPECT_DOUBLE_EQ(kmph2mps(36), 10); @@ -56,7 +56,7 @@ TEST(unit_conversion, kmph2mps) TEST(unit_conversion, mps2kmph) { - using autoware_universe_utils::mps2kmph; + using autoware::universe_utils::mps2kmph; EXPECT_DOUBLE_EQ(mps2kmph(0), 0); EXPECT_DOUBLE_EQ(mps2kmph(10), 36); diff --git a/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp index 238a97e901611..f9c0bd45c4fb4 100644 --- a/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp @@ -24,7 +24,7 @@ class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test { protected: std::shared_ptr node_{nullptr}; - std::shared_ptr published_time_publisher_ptr_{ + std::shared_ptr published_time_publisher_ptr_{ nullptr}; std::shared_ptr> first_test_publisher_ptr_{nullptr}; @@ -70,7 +70,7 @@ class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test // Create a PublishedTimePublisher published_time_publisher_ptr_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); // Create the first subscriber first_test_subscriber_ptr_ = @@ -98,7 +98,7 @@ class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test { protected: std::shared_ptr node_{nullptr}; - std::shared_ptr published_time_publisher_ptr_{ + std::shared_ptr published_time_publisher_ptr_{ nullptr}; std::shared_ptr> first_test_publisher_ptr_{nullptr}; @@ -135,7 +135,7 @@ class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test // Create a PublishedTimePublisher published_time_publisher_ptr_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); rclcpp::spin_some(node_); } diff --git a/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp b/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp index 6bf9857595f98..55e10e9bd2ffd 100644 --- a/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp +++ b/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp @@ -21,7 +21,7 @@ TEST(system, StopWatch_sec) { - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::StopWatch; StopWatch stop_watch; @@ -50,7 +50,7 @@ TEST(system, StopWatch_sec) TEST(system, StopWatch_msec) { - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::StopWatch; StopWatch stop_watch; diff --git a/common/autoware_universe_utils/test/src/transform/test_transform.cpp b/common/autoware_universe_utils/test/src/transform/test_transform.cpp index e659ec0f2a749..2935600c9f446 100644 --- a/common/autoware_universe_utils/test/src/transform/test_transform.cpp +++ b/common/autoware_universe_utils/test/src/transform/test_transform.cpp @@ -31,7 +31,7 @@ TEST(system, transform_point_cloud) 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; pcl::PointCloud cloud_transformed; - autoware_universe_utils::transformPointCloud(cloud, cloud_transformed, transform); + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform); pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4); @@ -53,8 +53,8 @@ TEST(system, empty_point_cloud) pcl::PointCloud cloud_transformed; EXPECT_NO_THROW( - autoware_universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); EXPECT_NO_FATAL_FAILURE( - autoware_universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); EXPECT_EQ(cloud_transformed.size(), 0ul); } diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index 370765a81c743..69c8840b39421 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -25,7 +25,7 @@ namespace goal_distance_calculator { -using autoware_universe_utils::PoseDeviation; +using autoware::universe_utils::PoseDeviation; struct Param { diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index c1154884e8340..037841a505017 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -45,7 +45,7 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber rclcpp::Subscription::SharedPtr sub_initial_pose_; - autoware_universe_utils::SelfPoseListener self_pose_listener_; + autoware::universe_utils::SelfPoseListener self_pose_listener_; rclcpp::Subscription::SharedPtr sub_route_; // Data Buffer @@ -56,7 +56,7 @@ class GoalDistanceCalculatorNode : public rclcpp::Node void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); // Publisher - autoware_universe_utils::DebugPublisher debug_publisher_; + autoware::universe_utils::DebugPublisher debug_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp index 5491fe96c0848..a577d43675224 100755 --- a/common/goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - autoware_universe_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); + autoware::universe_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); return output; } diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index 7f4346dac63dc..8e4e4c90bc470 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -107,7 +107,7 @@ void GoalDistanceCalculatorNode::onTimer() output_ = goal_distance_calculator_->update(input_); { - using autoware_universe_utils::rad2deg; + using autoware::universe_utils::rad2deg; const auto & deviation = output_.goal_deviation; debug_publisher_.publish( diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index b2fc5c6c36e54..398d841c54710 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -53,7 +53,7 @@ class SplineInterpolationPoints2d { std::vector points_inner; for (const auto & p : points) { - points_inner.push_back(autoware_universe_utils::getPoint(p)); + points_inner.push_back(autoware::universe_utils::getPoint(p)); } calcSplineCoefficientsInner(points_inner); } diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 11bc407e4c511..4d6d1639f2ac7 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -118,7 +118,7 @@ geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( geometry_msgs::msg::Pose pose; pose.position = getSplineInterpolatedPoint(idx, s); pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + autoware::universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); return pose; } diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index 2d582e2318600..4013832220cd8 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-6; TEST(spline_interpolation, splineYawFromPoints) { - using autoware_universe_utils::createPoint; + using autoware::universe_utils::createPoint; { // straight std::vector points; @@ -96,7 +96,7 @@ TEST(spline_interpolation, splineYawFromPoints) TEST(spline_interpolation, SplineInterpolationPoints2d) { - using autoware_universe_utils::createPoint; + using autoware::universe_utils::createPoint; // curve std::vector points; @@ -199,8 +199,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { + using autoware::universe_utils::createPoint; using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_universe_utils::createPoint; std::vector points; points.push_back(createPoint(-2.0, -10.0, 0.0)); diff --git a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp index eb8a7b81dbfb9..ae50227df1166 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp @@ -28,7 +28,7 @@ namespace object_recognition_utils { -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::Polygon2d; // minimum area to avoid division by zero static const double MIN_AREA = 1e-6; @@ -37,7 +37,7 @@ inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon boost::geometry::model::multi_polygon union_polygons; boost::geometry::union_(source_polygon, target_polygon, union_polygons); - autoware_universe_utils::Polygon2d hull; + autoware::universe_utils::Polygon2d hull; boost::geometry::convex_hull(union_polygons, hull); return boost::geometry::area(hull); } @@ -67,9 +67,9 @@ inline double getUnionArea(const Polygon2d & source_polygon, const Polygon2d & t template double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01) { - const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -84,9 +84,9 @@ double get2dIoU(const T1 source_object, const T2 target_object, const double min template double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) { - const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -100,10 +100,10 @@ double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) template double get2dPrecision(const T1 source_object, const T2 target_object) { - const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); const double source_area = boost::geometry::area(source_polygon); if (source_area < MIN_AREA) return 0.0; - const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -115,9 +115,9 @@ double get2dPrecision(const T1 source_object, const T2 target_object) template double get2dRecall(const T1 source_object, const T2 target_object) { - const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); const double target_area = boost::geometry::area(target_polygon); if (target_area < MIN_AREA) return 0.0; diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index e748d1165bef7..e26c9e6a7e1ea 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -38,7 +38,7 @@ boost::optional calcInterpolatedPose( if (relative_time - epsilon < time_step * path_idx) { const double offset = relative_time - time_step * (path_idx - 1); const double ratio = std::clamp(offset / time_step, 0.0, 1.0); - return autoware_universe_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); + return autoware::universe_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); } } @@ -90,7 +90,7 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( // Set Position for (size_t i = 0; i < resampled_size; ++i) { - const auto p = autoware_universe_utils::createPoint( + const auto p = autoware::universe_utils::createPoint( interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); resampled_path.path.at(i).position = p; resampled_path.path.at(i).orientation = interpolated_quat.at(i); diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/object_recognition_utils/test/src/test_matching.cpp index 1481492e40e71..6005ac8d1efbc 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/object_recognition_utils/test/src/test_matching.cpp @@ -19,8 +19,8 @@ #include -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Point3d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; constexpr double epsilon = 1e-06; @@ -30,7 +30,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double { geometry_msgs::msg::Pose p; p.position = geometry_msgs::build().x(x).y(y).z(0.0); - p.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + p.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return p; } } // namespace diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index 94136c9c4da9c..305a1173acf12 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -18,17 +18,17 @@ #include -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Point3d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; constexpr double epsilon = 1e-06; namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; using autoware_perception_msgs::msg::PredictedPath; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromRPY; -using autoware_universe_utils::transformPoint; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -62,9 +62,9 @@ PredictedPath createTestPredictedPath( TEST(predicted_path_utils, testCalcInterpolatedPose) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::createQuaternionFromYaw; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; using object_recognition_utils::calcInterpolatedPose; const auto path = createTestPredictedPath(100, 0.1, 1.0); @@ -128,9 +128,9 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) TEST(predicted_path_utils, resamplePredictedPath_by_vector) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::createQuaternionFromYaw; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; using object_recognition_utils::resamplePredictedPath; const auto path = createTestPredictedPath(10, 1.0, 1.0); @@ -205,9 +205,9 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) { - using autoware_universe_utils::createQuaternionFromRPY; - using autoware_universe_utils::createQuaternionFromYaw; - using autoware_universe_utils::deg2rad; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; using object_recognition_utils::resamplePredictedPath; const auto path = createTestPredictedPath(10, 1.0, 1.0); diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index b503d926df53b..47dd8258de5bb 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -48,7 +48,7 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio return; } - const double distance = autoware_motion_utils::calcSignedArcLength( + const double distance = autoware::motion_utils::calcSignedArcLength( path->points, pose->pose.position, path->points.size() - 1); tier4_debug_msgs::msg::Float64Stamped msg; diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp index ceef07ca268c4..8e8fbf8d6a019 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -30,7 +30,7 @@ class PathDistanceCalculator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_path_; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; - autoware_universe_utils::SelfPoseListener self_pose_listener_; + autoware::universe_utils::SelfPoseListener self_pose_listener_; autoware_planning_msgs::msg::Path::SharedPtr path_; }; diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index 6d7109fbfa061..ef5663f024c8d 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -61,22 +61,22 @@ void visualizeBound( const auto [normal_vector_angle, adaptive_width] = [&]() -> std::pair { if (i == 0) { return std::make_pair( - autoware_universe_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); + autoware::universe_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); } if (i == bound.size() - 1) { return std::make_pair( - autoware_universe_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); + autoware::universe_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); } const auto & prev_p = bound.at(i - 1); const auto & curr_p = bound.at(i); const auto & next_p = bound.at(i + 1); - const float curr_to_prev_angle = autoware_universe_utils::calcAzimuthAngle(curr_p, prev_p); - const float curr_to_next_angle = autoware_universe_utils::calcAzimuthAngle(curr_p, next_p); + const float curr_to_prev_angle = autoware::universe_utils::calcAzimuthAngle(curr_p, prev_p); + const float curr_to_next_angle = autoware::universe_utils::calcAzimuthAngle(curr_p, next_p); const float normal_vector_angle = (curr_to_prev_angle + curr_to_next_angle) / 2.0; const float diff_angle = - autoware_universe_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); + autoware::universe_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); if (diff_angle == 0.0) { return std::make_pair(normal_vector_angle, width); } diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 8edae446c46b0..25f890799465d 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -86,8 +86,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) { for (auto && path_point : msg_ptr->points) { if ( - !rviz_common::validateFloats(autoware_universe_utils::getPose(path_point)) && - !rviz_common::validateFloats(autoware_universe_utils::getLongitudinalVelocity(path_point))) { + !rviz_common::validateFloats(autoware::universe_utils::getPose(path_point)) && + !rviz_common::validateFloats(autoware::universe_utils::getLongitudinalVelocity(path_point))) { return false; } } @@ -358,8 +358,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); - const auto & pose = autoware_universe_utils::getPose(path_point); - const auto & velocity = autoware_universe_utils::getLongitudinalVelocity(path_point); + const auto & pose = autoware::universe_utils::getPose(path_point); + const auto & velocity = autoware::universe_utils::getLongitudinalVelocity(path_point); // path if (property_path_view_.getBool()) { @@ -454,9 +454,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx; const auto & prev_path_pos = - autoware_universe_utils::getPose(msg_ptr->points.at(prev_idx)).position; + autoware::universe_utils::getPose(msg_ptr->points.at(prev_idx)).position; const auto & next_path_pos = - autoware_universe_utils::getPose(msg_ptr->points.at(next_idx)).position; + autoware::universe_utils::getPose(msg_ptr->points.at(next_idx)).position; Ogre::Vector3 position; position.x = pose.position.x; @@ -467,7 +467,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_rendering::MovableText * text = slope_texts_.at(point_idx); const double slope = - autoware_universe_utils::calcElevationAngle(prev_path_pos, next_path_pos); + autoware::universe_utils::calcElevationAngle(prev_path_pos, next_path_pos); std::stringstream ss; ss << std::fixed << std::setprecision(2) << slope; @@ -511,7 +511,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { const auto & point = msg_ptr->points.at(p_idx); - const auto & pose = autoware_universe_utils::getPose(point); + const auto & pose = autoware::universe_utils::getPose(point); // footprint if (property_footprint_view_.getBool()) { Ogre::ColourValue color; diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp index c82f5b5df3885..7e99af8b9efb6 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp @@ -29,7 +29,7 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) // 1. check velocity const double target_velocity = - autoware_universe_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); + autoware::universe_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); if (epsilon < target_velocity) { return true; } else if (target_velocity < -epsilon) { @@ -46,13 +46,13 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) const size_t first_idx = is_last_point ? target_idx - 1 : target_idx; const size_t second_idx = is_last_point ? target_idx : target_idx + 1; - const auto first_pose = autoware_universe_utils::getPose(points_with_twist.at(first_idx)); - const auto second_pose = autoware_universe_utils::getPose(points_with_twist.at(second_idx)); + const auto first_pose = autoware::universe_utils::getPose(points_with_twist.at(first_idx)); + const auto second_pose = autoware::universe_utils::getPose(points_with_twist.at(second_idx)); const double first_traj_yaw = tf2::getYaw(first_pose.orientation); const double driving_direction_yaw = - autoware_universe_utils::calcAzimuthAngle(first_pose.position, second_pose.position); + autoware::universe_utils::calcAzimuthAngle(first_pose.position, second_pose.position); if ( - std::abs(autoware_universe_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < + std::abs(autoware::universe_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < M_PI_2) { return true; } diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp index a77e5769f7ff8..1d884d01065fa 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp @@ -231,14 +231,14 @@ void AccelerationMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp index 86adc54306262..03fef97f536a5 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -67,8 +67,8 @@ private Q_SLOTS: private: static constexpr float meter_min_acceleration_ = -10.0f; static constexpr float meter_max_acceleration_ = 10.0f; - static constexpr float meter_min_angle_ = autoware_universe_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = autoware_universe_utils::deg2rad(320.f); + static constexpr float meter_min_angle_ = autoware::universe_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware::universe_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 90bd6e91f9e4a..8f7348c87be3c 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -212,14 +212,14 @@ void ConsoleMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index fc54208ddb8c8..98cf8bbae4228 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -63,10 +63,10 @@ private Q_SLOTS: // QImage hud_; private: - static constexpr float meter_min_velocity_ = autoware_universe_utils::kmph2mps(0.f); - static constexpr float meter_max_velocity_ = autoware_universe_utils::kmph2mps(60.f); - static constexpr float meter_min_angle_ = autoware_universe_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = autoware_universe_utils::deg2rad(320.f); + static constexpr float meter_min_velocity_ = autoware::universe_utils::kmph2mps(0.f); + static constexpr float meter_max_velocity_ = autoware::universe_utils::kmph2mps(60.f); + static constexpr float meter_min_angle_ = autoware::universe_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware::universe_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine 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 1096def70d483..7e06af73b0f11 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 @@ -59,9 +59,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using visualization_msgs::msg::Marker; @@ -197,7 +197,7 @@ class CollisionDataKeeper const double p_vel = p_dist / p_dt; const auto nearest_idx = - autoware_motion_utils::findNearestIndex(path, nearest_collision_point); + 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) @@ -236,14 +236,14 @@ class AEB : public rclcpp::Node explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber - autoware_universe_utils::InterProcessPollingSubscriber sub_point_cloud_{ - this, "~/input/pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; - autoware_universe_utils::InterProcessPollingSubscriber sub_velocity_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_velocity_{ this, "~/input/velocity"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 29ba9474b6e8e..90cf3f900673e 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -70,33 +70,33 @@ Polygon2d createPolygon( appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); - polygon = autoware_universe_utils::isClockwise(polygon) + polygon = autoware::universe_utils::isClockwise(polygon) ? polygon - : autoware_universe_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); @@ -168,7 +168,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult AEB::onParameter( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); @@ -494,8 +494,8 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) double curr_y = 0.0; double curr_yaw = 0.0; geometry_msgs::msg::Pose ini_pose; - ini_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, 0.0); - ini_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); + ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); + ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); if (std::abs(curr_v) < 0.1) { @@ -511,23 +511,23 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware_universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { continue; } path.push_back(current_pose); } // If path is shorter than minimum path length - while (autoware_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; geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware_universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { continue; } path.push_back(current_pose); @@ -624,13 +624,13 @@ void AEB::createObjectDataUsingPointCloudClusters( const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); const auto & p = first_point_of_path.position; - return autoware_universe_utils::createPoint(p.x, p.y, p.z); + return autoware::universe_utils::createPoint(p.x, p.y, p.z); }(); for (const auto & p : *points_belonging_to_cluster_hulls) { - const auto obj_position = autoware_universe_utils::createPoint(p.x, p.y, p.z); + const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = - autoware_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 @@ -692,36 +692,38 @@ void AEB::addMarker( const double color_r, const double color_g, const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers) { - auto path_marker = autoware_universe_utils::createDefaultMarker( + auto path_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, - autoware_universe_utils::createMarkerScale(0.2, 0.2, 0.2), - autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.2), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); path_marker.points.resize(path.size()); for (size_t i = 0; i < path.size(); ++i) { path_marker.points.at(i) = path.at(i).position; } debug_markers.markers.push_back(path_marker); - auto polygon_marker = autoware_universe_utils::createDefaultMarker( + auto polygon_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, - autoware_universe_utils::createMarkerScale(0.03, 0.0, 0.0), - autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto & poly : polygons) { for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { const auto & boost_cp = poly.outer().at(dp_idx); const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); - const auto curr_point = autoware_universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); - const auto next_point = autoware_universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); polygon_marker.points.push_back(curr_point); polygon_marker.points.push_back(next_point); } } debug_markers.markers.push_back(polygon_marker); - auto object_data_marker = autoware_universe_utils::createDefaultMarker( + auto object_data_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, - autoware_universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto & e : objects) { object_data_marker.points.push_back(e.position); } @@ -730,11 +732,11 @@ void AEB::addMarker( // Visualize planner type text if (closest_object.has_value()) { const auto & obj = closest_object.value(); - const auto color = autoware_universe_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); - auto closest_object_velocity_marker_array = autoware_universe_utils::createDefaultMarker( + const auto color = autoware::universe_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); + auto closest_object_velocity_marker_array = autoware::universe_utils::createDefaultMarker( "base_link", obj.stamp, ns + "_closest_object_velocity", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware_universe_utils::createMarkerScale(0.0, 0.0, 0.7), color); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 0.7), color); closest_object_velocity_marker_array.pose.position = obj.position; const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; closest_object_velocity_marker_array.text = @@ -748,10 +750,10 @@ void AEB::addMarker( void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) { - auto point_marker = autoware_universe_utils::createDefaultMarker( + auto point_marker = autoware::universe_utils::createDefaultMarker( "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.3, 0.3, 0.3), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); } diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 4434dc23ebf96..ecb46aee123e3 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -69,9 +69,9 @@ class ControlValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); rclcpp::Subscription::SharedPtr sub_predicted_traj_; - autoware_universe_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_reference_traj_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_reference_traj_{ this, "~/input/reference_trajectory"}; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; 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 d64c718df3887..375ad557d02df 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp @@ -26,8 +26,8 @@ namespace autoware::control_validator { -using autoware_motion_utils::convertToTrajectory; -using autoware_motion_utils::convertToTrajectoryPointArray; +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; diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp index 7fdd447f075df..d5d8644515ab1 100644 --- a/control/autoware_control_validator/src/debug_marker.cpp +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -48,7 +48,7 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( void ControlValidatorDebugMarkerPublisher::pushPoseMarker( const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) { - using autoware_universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerColor; // append arrow marker std_msgs::msg::ColorRGBA color; @@ -64,9 +64,9 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( { color = createMarkerColor(0.0, 0.0, 1.0, 0.999); } - Marker marker = autoware_universe_utils::createDefaultMarker( + Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - autoware_universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); + autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; @@ -76,10 +76,10 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( void ControlValidatorDebugMarkerPublisher::pushWarningMsg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = autoware_universe_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - autoware_universe_utils::createMarkerScale(0.0, 0.0, 1.0), - autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -90,8 +90,8 @@ void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs:: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - autoware_motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); - autoware_universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + autoware::motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); + autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } void ControlValidatorDebugMarkerPublisher::publish() diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp index e60c6ea723b4f..821d64c3af6e6 100644 --- a/control/autoware_control_validator/src/utils.cpp +++ b/control/autoware_control_validator/src/utils.cpp @@ -37,7 +37,7 @@ void insertPointInPredictedTrajectory( TrajectoryPoints & modified_trajectory, const Pose & reference_pose, const TrajectoryPoints & predicted_trajectory) { - const auto point_to_interpolate = autoware_motion_utils::calcInterpolatedPoint( + const auto point_to_interpolate = autoware::motion_utils::calcInterpolatedPoint( convertToTrajectory(predicted_trajectory), reference_pose); modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); } @@ -59,7 +59,7 @@ bool removeFrontTrajectoryPoint( bool predicted_trajectory_point_removed = false; for (const auto & point : predicted_trajectory_points) { if ( - autoware_motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( trajectory_points, 0, point.pose.position) < 0.0) { modified_trajectory_points.erase(modified_trajectory_points.begin()); @@ -75,7 +75,7 @@ bool removeFrontTrajectoryPoint( Trajectory alignTrajectoryWithReferenceTrajectory( const Trajectory & trajectory, const Trajectory & predicted_trajectory) { - const auto last_seg_length = autoware_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 = - autoware_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 = autoware_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 > @@ -149,11 +149,11 @@ double calcMaxLateralDistance( alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); double max_dist = 0; for (const auto & point : alined_predicted_trajectory.points) { - const auto p0 = autoware_universe_utils::getPoint(point); + const auto p0 = autoware::universe_utils::getPoint(point); // find nearest segment const size_t nearest_segment_idx = - autoware_motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); - const double temp_dist = std::abs(autoware_motion_utils::calcLateralOffset( + 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_joy_controller/include/autoware/joy_controller/joy_controller.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp index cea29ad887121..fd2d89f446dbb 100644 --- a/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp @@ -70,9 +70,9 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - autoware_universe_utils::InterProcessPollingSubscriber sub_joy_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_joy_{ this, "input/joy"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "input/odometry"}; rclcpp::Time last_joy_received_time_; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index b5df59a45038c..b43a7d0d0369a 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -47,12 +47,12 @@ namespace autoware::lane_departure_checker { +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::PoseDeviation; +using autoware::universe_utils::Segment2d; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::PoseDeviation; -using autoware_universe_utils::Segment2d; using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; @@ -122,7 +122,7 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool checkPathWillLeaveLane( diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index 7f3fad0be4d28..e6421b2af84bb 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -67,15 +67,15 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; - autoware_universe_utils::InterProcessPollingSubscriber sub_route_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_route_{ this, "~/input/route"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ this, "~/input/reference_trajectory"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ this, "~/input/predicted_trajectory"}; // Data Buffer @@ -99,8 +99,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); // Publisher - autoware_universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - autoware_universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; + autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; // Timer rclcpp::TimerBase::SharedPtr timer_; 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 84de8441c5a40..132eba8dde0c9 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 @@ -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 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; namespace { @@ -102,7 +102,7 @@ Output LaneDepartureChecker::update(const Input & input) { Output output{}; - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; output.trajectory_deviation = calcTrajectoryDeviation( *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, @@ -169,9 +169,9 @@ PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { - const auto nearest_idx = autoware_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); + return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); } TrajectoryPoints LaneDepartureChecker::resampleTrajectory( @@ -183,8 +183,8 @@ TrajectoryPoints LaneDepartureChecker::resampleTrajectory( for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware_universe_utils::fromMsg(resampled.back().pose.position); - const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(resampled.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); if (boost::geometry::distance(p1.to_2d(), p2.to_2d()) > interval) { resampled.push_back(point); @@ -205,8 +205,8 @@ TrajectoryPoints LaneDepartureChecker::cutTrajectory( for (size_t i = 1; i < trajectory.size(); ++i) { const auto & point = trajectory.at(i); - const auto p1 = autoware_universe_utils::fromMsg(cut.back().pose.position); - const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(cut.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -251,7 +251,7 @@ std::vector LaneDepartureChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : trajectory) { vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, autoware_universe_utils::pose2transform(p.pose))); + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -268,7 +268,7 @@ std::vector LaneDepartureChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : path.points) { vehicle_footprints.push_back(transformVector( - local_vehicle_footprint, autoware_universe_utils::pose2transform(p.point.pose))); + local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose))); } return vehicle_footprints; @@ -322,18 +322,18 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); auto to_polygon2d = - [](const lanelet::BasicPolygon2d & poly) -> autoware_universe_utils::Polygon2d { - autoware_universe_utils::Polygon2d p; + [](const lanelet::BasicPolygon2d & poly) -> autoware::universe_utils::Polygon2d { + autoware::universe_utils::Polygon2d p; auto & outer = p.outer(); for (const auto & p : poly) { - autoware_universe_utils::Point2d p2d(p.x(), p.y()); + autoware::universe_utils::Point2d p2d(p.x(), p.y()); outer.push_back(p2d); } boost::geometry::correct(p); @@ -341,15 +341,15 @@ LaneDepartureChecker::getFusedLaneletPolygonForPath( }; // Fuse lanelets into a single BasicPolygon2d - auto fused_lanelets = [&]() -> std::optional { + auto fused_lanelets = [&]() -> std::optional { if (lanelets_distance_pair.empty()) return std::nullopt; - autoware_universe_utils::MultiPolygon2d lanelet_unions; - autoware_universe_utils::MultiPolygon2d result; + autoware::universe_utils::MultiPolygon2d lanelet_unions; + autoware::universe_utils::MultiPolygon2d result; for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; const auto & p = route_lanelet.polygon2d().basicPolygon(); - autoware_universe_utils::Polygon2d poly = to_polygon2d(p); + autoware::universe_utils::Polygon2d poly = to_polygon2d(p); boost::geometry::union_(lanelet_unions, poly, result); lanelet_unions = result; result.clear(); diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 84ab3877fe9f9..427cf0898470e 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -31,7 +31,7 @@ #include #include -using autoware_universe_utils::rad2deg; +using autoware::universe_utils::rad2deg; namespace { @@ -251,7 +251,7 @@ bool LaneDepartureCheckerNode::isDataValid() void LaneDepartureCheckerNode::onTimer() { std::map processing_time_map; - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("Total"); current_odom_ = sub_odom_.takeData(); @@ -453,9 +453,9 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation( visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const { - using autoware_universe_utils::createDefaultMarker; - using autoware_universe_utils::createMarkerColor; - using autoware_universe_utils::createMarkerScale; + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray marker_array; diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp index 2edbfdd3e1e90..b761aa9f19d23 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp @@ -117,7 +117,7 @@ class MPCTrajectory point.pose.position.x = x.at(i); point.pose.position.y = y.at(i); point.pose.position.z = z.at(i); - point.pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw.at(i)); + point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw.at(i)); point.longitudinal_velocity_mps = vx.at(i); points.push_back(point); } diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 309704596086e..1f3327bf28f2e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -25,9 +25,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::normalizeRadian; -using autoware_universe_utils::rad2deg; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::normalizeRadian; +using autoware::universe_utils::rad2deg; MPC::MPC(rclcpp::Node & node) { @@ -167,10 +167,10 @@ void MPC::setReferenceTrajectory( const Odometry & current_kinematics) { const size_t nearest_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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( + 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); @@ -184,7 +184,7 @@ void MPC::setReferenceTrajectory( } const auto is_forward_shift = - autoware_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; @@ -391,7 +391,7 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter( } const size_t nearest_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); @@ -501,7 +501,7 @@ MPCMatrix MPC::generateMPCMatrix( // get reference input (feed-forward) m_vehicle_model_ptr->setCurvature(ref_smooth_k); m_vehicle_model_ptr->calculateReferenceInput(Uref); - if (std::fabs(Uref(0, 0)) < autoware_universe_utils::deg2rad(m_param.zero_ff_steer_deg)) { + if (std::fabs(Uref(0, 0)) < autoware::universe_utils::deg2rad(m_param.zero_ff_steer_deg)) { Uref(0, 0) = 0.0; // ignore curvature noise } m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; @@ -678,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 = autoware_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 6aedb74e87c83..7634ebb74eca7 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -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 = autoware_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); @@ -616,7 +616,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const // TODO(Horibe): update implementation to check trajectory shape around ego vehicle. // Now temporally check the goal position. for (const auto & trajectory : m_trajectory_buffer) { - const auto change_distance = autoware_universe_utils::calcDistance2d( + const auto change_distance = autoware::universe_utils::calcDistance2d( trajectory.points.back().pose, m_current_trajectory.points.back().pose); if (change_distance > m_new_traj_end_dist) { return true; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index a2dd346a442b3..37eb47ab0396e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -42,9 +42,9 @@ double calcLongitudinalOffset( namespace MPCUtils { -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::createQuaternionFromYaw; -using autoware_universe_utils::normalizeRadian; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::createQuaternionFromYaw; +using autoware::universe_utils::normalizeRadian; double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2) { @@ -240,7 +240,7 @@ std::vector calcTrajectoryCurvature( p2.y = traj.y.at(curr_idx); p3.y = traj.y.at(next_idx); try { - curvature_vec.at(curr_idx) = autoware_universe_utils::calcCurvature(p1, p2, p3); + curvature_vec.at(curr_idx) = autoware::universe_utils::calcCurvature(p1, p2, p3); } catch (...) { std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl; curvature_vec.at(curr_idx) = 0.0; @@ -281,7 +281,7 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input) p.pose.position.x = input.x.at(i); p.pose.position.y = input.y.at(i); p.pose.position.z = input.z.at(i); - p.pose.orientation = autoware_universe_utils::createQuaternionFromYaw(input.yaw.at(i)); + p.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(input.yaw.at(i)); p.longitudinal_velocity_mps = static_cast(input.vx.at(i)); output.points.push_back(p); @@ -346,7 +346,7 @@ bool calcNearestPoseInterp( return false; } - *nearest_index = autoware_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(); @@ -389,7 +389,7 @@ bool calcNearestPoseInterp( prev_traj_point.x = traj.x.at(prev); prev_traj_point.y = traj.y.at(prev); const double traj_seg_length = - autoware_universe_utils::calcDistance2d(prev_traj_point, next_traj_point); + autoware::universe_utils::calcDistance2d(prev_traj_point, next_traj_point); /* if distance between two points are too close */ if (traj_seg_length < 1.0E-5) { nearest_pose->position.x = traj.x.at(*nearest_index); @@ -460,7 +460,7 @@ void extendTrajectoryInYawDirection( const double dt = interval / extend_vel; const size_t num_extended_point = static_cast(extend_dist / interval); for (size_t i = 0; i < num_extended_point; ++i) { - extended_pose = autoware_universe_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); + extended_pose = autoware::universe_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); traj.push_back( extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(), extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt); diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp index 46cc360355e67..d69d79f9640d8 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.hpp +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -49,9 +49,9 @@ class OperationModeTransitionManager : public rclcpp::Node const ChangeOperationModeAPI::Service::Response::SharedPtr response); using ControlModeCommandType = ControlModeCommand::Request::_mode_type; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_control_mode_report_{this, "control_mode_report"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_gate_operation_mode_{this, "gate_operation_mode"}; rclcpp::Client::SharedPtr cli_control_mode_; rclcpp::Publisher::SharedPtr pub_debug_info_; diff --git a/control/autoware_operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp index ea2f9bf4bf03c..31cd32e6311a9 100644 --- a/control/autoware_operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -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 autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcYawDeviation; 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 = - autoware_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 = - autoware_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 f0e231e354bc4..a36a0b4eefccd 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 @@ -87,13 +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 = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, pose, max_dist, max_yaw); const double len_to_interpolated = - autoware_motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); const double len_segment = - autoware_motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1); + 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/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index ed9fcb3974c83..9291d538b022f 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -50,10 +50,10 @@ namespace autoware::motion::control::pid_longitudinal_controller { +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; using visualization_msgs::msg::Marker; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; 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 2a897f92a0efe..5eb6c87e063eb 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 = autoware_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 = autoware_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 = autoware_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)); @@ -93,7 +93,7 @@ double getPitchByTraj( const auto [prev_idx, next_idx] = [&]() { for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = autoware_universe_utils::calcDistance3d( + const double dist = autoware::universe_utils::calcDistance3d( trajectory.points.at(start_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) @@ -105,7 +105,7 @@ double getPitchByTraj( std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); }(); - return autoware_universe_utils::calcElevationAngle( + return autoware::universe_utils::calcElevationAngle( trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position); } @@ -166,7 +166,7 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( double remain_dist = distance; geometry_msgs::msg::Pose p = trajectory.points.back().pose; for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) { - const double dist = autoware_universe_utils::calcDistance3d( + const double dist = autoware::universe_utils::calcDistance3d( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose); if (remain_dist < dist) { if (remain_dist <= 0.0) { 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 dda6b9632a769..78c7ccf832514 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -465,10 +465,10 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // check if the deviation is worth emergency m_diagnostic_data.trans_deviation = - autoware_universe_utils::calcDistance2d(current_interpolated_pose.first, current_pose); + autoware::universe_utils::calcDistance2d(current_interpolated_pose.first, current_pose); const bool is_dist_deviation_large = m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; - m_diagnostic_data.rot_deviation = std::abs(autoware_universe_utils::normalizeRadian( + m_diagnostic_data.rot_deviation = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(current_interpolated_pose.first.pose.orientation) - tf2::getYaw(current_pose.orientation))); const bool is_yaw_deviation_large = @@ -509,11 +509,11 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // ========================================================================================== // Remove overlapped points after inserting the interpolated points control_data.interpolated_traj.points = - autoware_motion_utils::removeOverlapPoints(control_data.interpolated_traj.points); - control_data.nearest_idx = autoware_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 = autoware_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 autoware_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 @@ -605,7 +605,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d auto marker = createDefaultMarker( "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = autoware_universe_utils::calcOffsetPose( + marker.pose = autoware::universe_utils::calcOffsetPose( m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, m_vehicle_width / 2 + 2.0, 1.5); marker.text = "steering not\nconverged"; @@ -971,7 +971,7 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop } const auto traj = control_data.interpolated_traj; - const auto stop_idx = autoware_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_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp index 1030b58e0f979..58ce03a0dbf26 100644 --- a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp @@ -78,10 +78,10 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber - autoware_universe_utils::SelfPoseListener self_pose_listener_{this}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::SelfPoseListener self_pose_listener_{this}; + autoware::universe_utils::InterProcessPollingSubscriber sub_trajectory_{this, "input/reference_trajectory"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{this, "input/current_odometry"}; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; 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 8a82e60354f36..803a6ef1e8617 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 @@ -141,7 +141,7 @@ TrajectoryPoint PurePursuitLateralController::calcNextPose( const double ds, TrajectoryPoint & point, Lateral cmd) const { geometry_msgs::msg::Transform transform; - transform.translation = autoware_universe_utils::createTranslation(ds, 0.0, 0.0); + transform.translation = autoware::universe_utils::createTranslation(ds, 0.0, 0.0); transform.rotation = planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); TrajectoryPoint output_p; @@ -158,17 +158,17 @@ void PurePursuitLateralController::setResampledTrajectory() { // Interpolate with constant interval distance. std::vector out_arclength; - const auto input_tp_array = autoware_motion_utils::convertToTrajectoryPointArray(trajectory_); - const auto traj_length = autoware_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( - autoware_motion_utils::resampleTrajectory( - autoware_motion_utils::convertToTrajectory(input_tp_array), out_arclength)); + 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_ = autoware_motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); + output_tp_array_ = autoware::motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); } double PurePursuitLateralController::calcCurvature(const size_t closest_idx) @@ -202,10 +202,10 @@ double PurePursuitLateralController::calcCurvature(const size_t closest_idx) double current_curvature = 0.0; try { - current_curvature = autoware_universe_utils::calcCurvature( - autoware_universe_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), - autoware_universe_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), - autoware_universe_utils::getPoint(trajectory_resampled_->points.at(next_idx))); + current_curvature = autoware::universe_utils::calcCurvature( + autoware::universe_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), + autoware::universe_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), + autoware::universe_utils::getPoint(trajectory_resampled_->points.at(next_idx))); } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what()); @@ -268,7 +268,7 @@ void PurePursuitLateralController::averageFilterTrajectory( boost::optional PurePursuitLateralController::generatePredictedTrajectory() { - const auto closest_idx_result = autoware_motion_utils::findNearestIndex( + 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) { @@ -427,7 +427,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( // Calculate target point for velocity/acceleration const auto closest_idx_result = - autoware_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 = - autoware_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_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp index cffdbf2cdf2ab..c7ce822d1ac18 100644 --- a/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp +++ b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp @@ -43,11 +43,11 @@ class ShiftDecider : public rclcpp::Node void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_control_cmd_{this, "input/control_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{this, "input/state"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_current_gear_{this, "input/current_gear"}; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp index 06873c31cc6cc..916f921a40e7b 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -54,8 +54,8 @@ using trajectory_follower::LongitudinalOutput; namespace trajectory_follower_node { +using autoware::universe_utils::StopWatch; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -77,20 +77,21 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::shared_ptr lateral_controller_; // Subscribers - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_ref_path_{this, "~/input/reference_trajectory"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/current_odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_vehicle_msgs::msg::SteeringReport> sub_steering_{this, "~/input/current_steering"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_accel_{this, "~/input/current_accel"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/current_operation_mode"}; // Publishers @@ -129,9 +130,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp index 0c179ad1996f4..c23128ebfb695 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp @@ -44,9 +44,9 @@ class SimpleTrajectoryFollower : public rclcpp::Node ~SimpleTrajectoryFollower() = default; private: - autoware_universe_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_trajectory_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_trajectory_{ this, "~/input/trajectory"}; rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index 7876a38247bc9..60f7481f862a9 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -80,10 +80,10 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( @@ -223,10 +223,10 @@ void Controller::publishDebugMarker( // steer converged marker { - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware_universe_utils::createMarkerScale(0.0, 0.0, 1.0), - autoware_universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); marker.pose = input_data.current_odometry.pose.pose; std::stringstream ss; 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 81459dc9ef72f..95082e608d50f 100644 --- a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -22,9 +22,9 @@ namespace simple_trajectory_follower { -using autoware_motion_utils::findNearestIndex; -using autoware_universe_utils::calcLateralDeviation; -using autoware_universe_utils::calcYawDeviation; +using autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcLateralDeviation; +using autoware::universe_utils::calcYawDeviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 7295032ae3cb7..87e79f59bc356 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -210,10 +210,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) timer_pub_status_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); // Parameter Callback set_param_res_ = @@ -223,7 +223,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; // Parameter updateParam(parameters, "use_emergency_handling", use_emergency_handling_); updateParam( 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 08de508dbefcc..42e28d633d16b 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -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 autoware_motion_utils::VehicleStopChecker; +using autoware::motion_utils::VehicleStopChecker; struct Commands { Control control; @@ -155,31 +155,31 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ this, "input/auto/gear_cmd"}; void onAutoCtrlCmd(Control::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ this, "input/external/gear_cmd"}; void onRemoteCtrlCmd(Control::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ this, "input/emergency/gear_cmd"}; void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); @@ -261,9 +261,9 @@ class VehicleCmdGate : public rclcpp::Node MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::vehicle_cmd_gate 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 e72adc3ecdad4..5f72c8ea316bd 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -86,7 +86,7 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - const auto closest_segment = autoware_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_); @@ -158,7 +158,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() // Compute the yaw angle error. const double heading_yaw_error = - autoware_universe_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); + autoware::universe_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); // Set the values of ErrorMsgVars. @@ -174,7 +174,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() const double Vx = odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; // Current acceleration calculation - const auto ds = autoware_universe_utils::calcDistance2d( + const auto ds = autoware::universe_utils::calcDistance2d( odom_history_ptr_->at(odom_size - 1).pose.pose, odom_history_ptr_->at(odom_size - 2).pose.pose); const double vel_mean = (odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x + @@ -372,7 +372,7 @@ std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() auto fun_distance_cond = [this](auto point_t) { const double dist = - autoware_universe_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); + autoware::universe_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); return dist > p_.wheelbase_; }; @@ -391,7 +391,7 @@ std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() std::pair ControlPerformanceAnalysisCore::calculateClosestPose() { const auto interp_point = - autoware_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()); @@ -443,7 +443,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // Compute arc-length ds between 2 points. const double ds_arc_length = - autoware_universe_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); + autoware::universe_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); // Define waypoints 10 meters behind the rear axle if exist. // If not exist, we will take the first point of the @@ -462,7 +462,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // We compute a curvature estimate from these points. double estimated_curvature = 0.0; try { - estimated_curvature = autoware_universe_utils::calcCurvature( + estimated_curvature = autoware::universe_utils::calcCurvature( points.at(loc_of_back_idx).pose.position, points.at(idx_curve_ref_wp).pose.position, points.at(loc_of_forward_idx).pose.position); } catch (...) { @@ -487,7 +487,7 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() const double look_ahead_distance_pp = std::max(p_.wheelbase_, 2 * Vx); auto fun_distance_cond = [this, &look_ahead_distance_pp](auto point_t) { - const double dist = autoware_universe_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); + const double dist = autoware::universe_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); return dist > look_ahead_distance_pp; }; diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index 27f4405e26f6c..8cce8b88a0e7f 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -35,7 +35,7 @@ namespace obstacle_collision_checker { -using autoware_universe_utils::LinearRing2d; +using autoware::universe_utils::LinearRing2d; struct Param { diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 85b3e3d25c2eb..b08ecccd57282 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -48,8 +48,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node private: // Subscriber - std::shared_ptr self_pose_listener_; - std::shared_ptr transform_listener_; + std::shared_ptr self_pose_listener_; + std::shared_ptr transform_listener_; rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; @@ -72,8 +72,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher - std::shared_ptr debug_publisher_; - std::shared_ptr time_publisher_; + std::shared_ptr debug_publisher_; + std::shared_ptr time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index e6c3aff83ee10..9fb3657b957c7 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -90,7 +90,7 @@ ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) Output ObstacleCollisionChecker::update(const Input & input) { Output output; - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; // resample trajectory by braking distance constexpr double min_velocity = 0.01; @@ -131,8 +131,9 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware_universe_utils::fromMsg(resampled.points.back().pose.position).to_2d(); - const auto p2 = autoware_universe_utils::fromMsg(point.pose.position).to_2d(); + const auto p1 = + autoware::universe_utils::fromMsg(resampled.points.back().pose.position).to_2d(); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position).to_2d(); if (boost::geometry::distance(p1, p2) > interval) { resampled.points.push_back(point); @@ -154,8 +155,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware_universe_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -197,8 +198,8 @@ std::vector ObstacleCollisionChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : trajectory.points) { vehicle_footprints.push_back( - autoware_universe_utils::transformVector( - local_vehicle_footprint, autoware_universe_utils::pose2transform(p.pose))); + autoware::universe_utils::transformVector( + local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -221,7 +222,7 @@ std::vector ObstacleCollisionChecker::createVehiclePassingAreas( LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( const LinearRing2d & area1, const LinearRing2d & area2) { - autoware_universe_utils::MultiPoint2d combined; + autoware::universe_utils::MultiPoint2d combined; for (const auto & p : area1) { combined.push_back(p); } @@ -256,7 +257,7 @@ bool ObstacleCollisionChecker::hasCollision( { for (const auto & point : obstacle_pointcloud.points) { if (boost::geometry::within( - autoware_universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) { + autoware::universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( rclcpp::get_logger("obstacle_collision_checker"), "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index 9bd95a985e2de..9af5fe24b17ea 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -70,8 +70,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt obstacle_collision_checker_->setParam(param_); // Subscriber - self_pose_listener_ = std::make_shared(this); - transform_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), @@ -87,8 +87,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt // Publisher debug_publisher_ = - std::make_shared(this, "debug/marker"); - time_publisher_ = std::make_shared(this); + std::make_shared(this, "debug/marker"); + time_publisher_ = std::make_shared(this); // Diagnostic Updater updater_.setHardwareID("obstacle_collision_checker"); @@ -283,9 +283,9 @@ void ObstacleCollisionCheckerNode::checkLaneDeparture( visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const { - using autoware_universe_utils::createDefaultMarker; - using autoware_universe_utils::createMarkerColor; - using autoware_universe_utils::createMarkerScale; + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray marker_array; 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 01223a4a938be..e2c76bec24860 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 @@ -45,10 +45,10 @@ namespace autoware::motion::control::predicted_path_checker using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using PointArray = std::vector; diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp index 84ccbad32202c..0f537d52cee06 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp @@ -52,12 +52,12 @@ class PredictedPathCheckerDebugNode ~PredictedPathCheckerDebugNode() {} bool pushPolygon( - const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); + const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); 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 5474bb93fbf4b..b3afcded60438 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 @@ -50,9 +50,9 @@ namespace autoware::motion::control::predicted_path_checker using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -88,7 +88,7 @@ class PredictedPathCheckerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_cli_; // Subscriber - std::shared_ptr self_pose_listener_; + std::shared_ptr self_pose_listener_; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; 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 05c7a5008db27..6f5c0e5cdb883 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -39,12 +39,12 @@ namespace utils { +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp index fb2eda3d04c33..7771f59d15454 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -80,12 +80,12 @@ CollisionChecker::checkTrajectoryForCollision( double distance_to_current = std::numeric_limits::max(); double distance_to_history = std::numeric_limits::max(); if (found_collision_at_dynamic_objects) { - distance_to_current = autoware_universe_utils::calcDistance2d( + distance_to_current = autoware::universe_utils::calcDistance2d( p_front, found_collision_at_dynamic_objects.get().first); } if (found_collision_at_history) { distance_to_history = - autoware_universe_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + autoware::universe_utils::calcDistance2d(p_front, found_collision_at_history.get().first); } else { predicted_object_history_.clear(); } @@ -140,7 +140,7 @@ CollisionChecker::checkObstacleHistory( bool is_init = false; std::pair nearest_collision_object_with_point; for (const auto & p : collision_points_in_history) { - double norm = autoware_universe_utils::calcDistance2d(p.first, base_pose); + double norm = autoware::universe_utils::calcDistance2d(p.first, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; nearest_collision_object_with_point = p; 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 bf74bf3c8bfc1..3fae5e38e7ede 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 @@ -28,15 +28,15 @@ #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; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; -using autoware_universe_utils::createPoint; +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; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; namespace autoware::motion::control::predicted_path_checker { @@ -51,7 +51,7 @@ PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( } bool PredictedPathCheckerDebugNode::pushPolygon( - const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; for (const auto & point : polygon.outer()) { @@ -82,7 +82,7 @@ bool PredictedPathCheckerDebugNode::pushPolygon( } bool PredictedPathCheckerDebugNode::pushPolyhedron( - const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; 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 524313181f36d..d8cf5c34bf535 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 @@ -67,7 +67,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n declare_parameter("collision_checker_params.chattering_threshold", 0.2); // Subscriber - self_pose_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); sub_dynamic_objects_ = create_subscription( "~/input/objects", rclcpp::SensorDataQoS(), @@ -242,8 +242,8 @@ void PredictedPathCheckerNode::onTimer() // Convert to trajectory array TrajectoryPoints predicted_trajectory_array = - autoware_motion_utils::convertToTrajectoryPointArray( - autoware_motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); + autoware::motion_utils::convertToTrajectoryPointArray( + autoware::motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); // Filter the objects @@ -323,7 +323,7 @@ void PredictedPathCheckerNode::onTimer() // trajectory or not const auto reference_trajectory_array = - autoware_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)); @@ -368,7 +368,7 @@ TrajectoryPoints PredictedPathCheckerNode::trimTrajectoryFromSelfPose( TrajectoryPoints output{}; const size_t min_distance_index = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( input, self_pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold) + 1; @@ -387,9 +387,9 @@ bool PredictedPathCheckerNode::isThereStopPointOnReferenceTrajectory( trimTrajectoryFromSelfPose(reference_trajectory_array, current_pose_.get()->pose); const auto nearest_stop_point_on_ref_trajectory = - autoware_motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); + autoware::motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); - const auto stop_point_on_trajectory = autoware_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; @@ -427,18 +427,18 @@ bool PredictedPathCheckerNode::isItDiscretePoint( const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const { const auto nearest_segment = - autoware_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); - const auto distance = autoware_universe_utils::calcDistance2d( + const auto distance = autoware::universe_utils::calcDistance2d( nearest_point.pose.position, collision_point.pose.position); const auto yaw_diff = - std::abs(autoware_universe_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + std::abs(autoware::universe_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); return distance >= node_param_.distinct_point_distance_threshold || - yaw_diff >= autoware_universe_utils::deg2rad(node_param_.distinct_point_yaw_threshold); + yaw_diff >= autoware::universe_utils::deg2rad(node_param_.distinct_point_yaw_threshold); } Trajectory PredictedPathCheckerNode::cutTrajectory( @@ -454,8 +454,8 @@ Trajectory PredictedPathCheckerNode::cutTrajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware_universe_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -482,7 +482,7 @@ Trajectory PredictedPathCheckerNode::cutTrajectory( cut.points.push_back(point); total_length += points_distance; } - autoware_motion_utils::removeOverlapPoints(cut.points); + autoware::motion_utils::removeOverlapPoints(cut.points); return cut; } @@ -501,7 +501,7 @@ size_t PredictedPathCheckerNode::insertStopPoint( TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point) { const auto nearest_collision_segment = - autoware_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); @@ -528,7 +528,7 @@ std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; const auto acceleration_obj = object.kinematics.initial_acceleration_with_covariance.accel.linear.x; - const auto k = std::cos(autoware_universe_utils::normalizeRadian( + const auto k = std::cos(autoware::universe_utils::normalizeRadian( tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); const auto projected_velocity = velocity_obj * k; const auto projected_acceleration = acceleration_obj * k; @@ -555,10 +555,10 @@ void PredictedPathCheckerNode::filterObstacles( // Check is it near to trajectory const double max_length = utils::calcObstacleMaxLength(object.shape); - const size_t seg_idx = autoware_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)); + 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)); const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; 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 0f4cc202f5c02..d750f3aa07248 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 autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; +using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::getRPY; // Utils Functions Polygon2d createOneStepPolygon( @@ -40,40 +40,40 @@ Polygon2d createOneStepPolygon( { // base step appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) .position); appendPointToPolygon( - polygon, autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) + polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) .position); appendPointToPolygon( - polygon, - autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0) + .position); } { // next step appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) .position); appendPointToPolygon( - polygon, autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) + polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) .position); appendPointToPolygon( - polygon, - autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0) + .position); } - polygon = autoware_universe_utils::isClockwise(polygon) + polygon = autoware::universe_utils::isClockwise(polygon) ? polygon - : autoware_universe_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); Polygon2d hull_polygon; boost::geometry::convex_hull(polygon, hull_polygon); @@ -97,8 +97,8 @@ TrajectoryPoint calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = trajectory.at(segment_idx); const auto & next_pt = trajectory.at(segment_idx + 1); - const auto v1 = autoware_universe_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = autoware_universe_utils::point2tfVector(curr_pt, target_point); + const auto v1 = autoware::universe_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = autoware::universe_utils::point2tfVector(curr_pt, target_point); if (v1.length2() < 1e-3) { return curr_pt; } @@ -111,7 +111,7 @@ TrajectoryPoint calcInterpolatedPoint( // pose interpolation interpolated_point.pose = - autoware_universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + autoware::universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -160,7 +160,7 @@ std::pair findStopPoint( for (size_t i = collision_idx; i > 0; i--) { distance_point_to_collision = - autoware_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; @@ -176,8 +176,8 @@ std::pair findStopPoint( base_point.pose.position.x - next_point.pose.position.x, base_point.pose.position.y - next_point.pose.position.y)); - geometry_msgs::msg::Pose interpolated_pose = - autoware_universe_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); + geometry_msgs::msg::Pose interpolated_pose = autoware::universe_utils::calcInterpolatedPose( + base_point.pose, next_point.pose, ratio, false); TrajectoryPoint output; output.set__pose(interpolated_pose); return std::make_pair(stop_segment_idx, output); @@ -196,7 +196,7 @@ bool isInBrakeDistance( return false; } - const auto distance_to_obstacle = autoware_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 + @@ -249,7 +249,7 @@ double getNearestPointAndDistanceForPredictedObject( bool is_init = false; for (const auto & p : points) { - double norm = autoware_universe_utils::calcDistance2d(p, base_pose); + double norm = autoware::universe_utils::calcDistance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; *nearest_collision_point = p; @@ -371,7 +371,7 @@ Polygon2d convertObjToPolygon(const PredictedObject & obj) bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) { - const auto yaw = autoware_universe_utils::getRPY(ego_pose).z; + const auto yaw = autoware::universe_utils::getRPY(ego_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); const Eigen::Vector2d obstacle_vec( obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); @@ -403,7 +403,7 @@ void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) { - const double yaw = autoware_universe_utils::getRPY( + const double yaw = autoware::universe_utils::getRPY( predicted_object.kinematics.initial_pose_with_covariance.pose.orientation) .z; const double vx = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; @@ -414,8 +414,8 @@ void getCurrentObjectPose( const double delta_yaw = predicted_object.kinematics.initial_twist_with_covariance.twist.angular.z * dt; geometry_msgs::msg::Transform transform; - transform.translation = autoware_universe_utils::createTranslation(ds, 0.0, 0.0); - transform.rotation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + transform.translation = autoware::universe_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); tf2::Transform tf_pose; tf2::Transform tf_offset; @@ -424,8 +424,8 @@ void getCurrentObjectPose( tf2::toMsg(tf_pose * tf_offset, predicted_object.kinematics.initial_pose_with_covariance.pose); predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x += ax * dt; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromRPY( - 0.0, 0.0, autoware_universe_utils::normalizeRadian(yaw + delta_yaw)); + autoware::universe_utils::createQuaternionFromRPY( + 0.0, 0.0, autoware::universe_utils::normalizeRadian(yaw + delta_yaw)); } } // namespace utils diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index efeb068fca115..279bada80e1b9 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -72,9 +72,9 @@ class controlEvaluatorNode : public rclcpp::Node // onDiagnostics(). rclcpp::Subscription::SharedPtr control_diag_sub_; - autoware_universe_utils::InterProcessPollingSubscriber odometry_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber traj_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index a0d8f4076dae4..689291da09f6d 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -26,15 +26,16 @@ using autoware_planning_msgs::msg::Trajectory; double calcLateralDeviation(const Trajectory & traj, const Point & point) { - const size_t nearest_index = autoware_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)); + autoware::universe_utils::calcLateralDeviation(traj.points[nearest_index].pose, point)); } double calcYawDeviation(const Trajectory & traj, const Pose & pose) { - 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)); + 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)); } } // namespace metrics diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index b7be356a7c522..8ce8a009652d8 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -38,8 +38,8 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra */ for (TrajectoryPoint p : traj.points) { const size_t nearest_index = - autoware_motion_utils::findNearestIndex(ref.points, p.pose.position); - stat.add(autoware_universe_utils::calcLateralDeviation( + autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); + stat.add(autoware::universe_utils::calcLateralDeviation( ref.points[nearest_index].pose, p.pose.position)); } return stat; @@ -58,8 +58,8 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) */ for (TrajectoryPoint p : traj.points) { 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)); + autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); + stat.add(autoware::universe_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); } return stat; } @@ -75,7 +75,7 @@ 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 = - autoware_motion_utils::findNearestIndex(ref.points, p.pose.position); + autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add(p.longitudinal_velocity_mps - ref.points[nearest_index].longitudinal_velocity_mps); } return stat; @@ -84,21 +84,21 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) { Stat stat; - stat.add(std::abs(autoware_universe_utils::calcLongitudinalDeviation(base_pose, target_point))); + stat.add(std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point))); return stat; } Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point) { Stat stat; - stat.add(std::abs(autoware_universe_utils::calcLateralDeviation(base_pose, target_point))); + stat.add(std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point))); return stat; } Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose) { Stat stat; - stat.add(std::abs(autoware_universe_utils::calcYawDeviation(base_pose, target_pose))); + stat.add(std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose))); return stat; } } // namespace metrics diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 906501b05e171..e17cfd98b27da 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -20,9 +20,9 @@ namespace metrics { namespace utils { +using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::calcDistance2d; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index 7836b0b6d694a..3cdaf3d7eaaae 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -27,8 +27,8 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::calcDistance2d; Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index 874f2113b7500..4d1c02faa406f 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -41,7 +41,8 @@ Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & tr for (size_t i = 0; i < traj1.points.size(); ++i) { for (size_t j = 0; j < traj2.points.size(); ++j) { - const double dist = autoware_universe_utils::calcDistance2d(traj1.points[i], traj2.points[j]); + const double dist = + autoware::universe_utils::calcDistance2d(traj1.points[i], traj2.points[j]); if (i > 0 && j > 0) { ca(i, j) = std::max(std::min(ca(i - 1, j), std::min(ca(i - 1, j - 1), ca(i, j - 1))), dist); } else if (i > 0 /*&& j == 0*/) { @@ -64,29 +65,29 @@ Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & tr return stat; } for (const auto & point : traj2.points) { - const auto p0 = autoware_universe_utils::getPoint(point); + const auto p0 = autoware::universe_utils::getPoint(point); // find nearest segment const size_t nearest_segment_idx = - autoware_motion_utils::findNearestSegmentIndex(traj1.points, p0); + autoware::motion_utils::findNearestSegmentIndex(traj1.points, p0); double dist; // distance to segment if ( nearest_segment_idx == traj1.points.size() - 2 && - autoware_motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( traj1.points, nearest_segment_idx, p0) > - autoware_universe_utils::calcDistance2d( + 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); + dist = autoware::universe_utils::calcDistance2d(traj1.points.back(), p0); } else if ( // NOLINT - nearest_segment_idx == 0 && autoware_motion_utils::calcLongitudinalOffsetToSegment( + 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); + dist = autoware::universe_utils::calcDistance2d(traj1.points.front(), p0); } else { // orthogonal distance - const auto p1 = autoware_universe_utils::getPoint(traj1.points[nearest_segment_idx]); - const auto p2 = autoware_universe_utils::getPoint(traj1.points[nearest_segment_idx + 1]); + const auto p1 = autoware::universe_utils::getPoint(traj1.points[nearest_segment_idx]); + const auto p2 = autoware::universe_utils::getPoint(traj1.points[nearest_segment_idx + 1]); dist = std::abs((p2.x - p1.x) * (p1.y - p0.y) - (p1.x - p0.x) * (p2.y - p1.y)) / std::sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y)); } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index 3f33447c1ce6b..4526865ced97d 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -20,8 +20,8 @@ namespace planning_diagnostics { namespace metrics { -using autoware_universe_utils::calcCurvature; -using autoware_universe_utils::calcDistance2d; +using autoware::universe_utils::calcCurvature; +using autoware::universe_utils::calcDistance2d; Stat calcTrajectoryInterval(const Trajectory & traj) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index d0e35fb24d534..8658a666b4976 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -123,7 +123,7 @@ Trajectory MetricsCalculator::getLookaheadTrajectory( } const auto ego_index = - autoware_motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); Trajectory lookahead_traj; lookahead_traj.header = traj.header; double dist = 0.0; @@ -132,7 +132,7 @@ Trajectory MetricsCalculator::getLookaheadTrajectory( auto prev_point_it = curr_point_it; while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { lookahead_traj.points.push_back(*curr_point_it); - const auto d = autoware_universe_utils::calcDistance2d( + const auto d = autoware::universe_utils::calcDistance2d( prev_point_it->pose.position, curr_point_it->pose.position); dist += d; if (prev_point_it->longitudinal_velocity_mps != 0.0) { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index 2d60ef826a5e9..9c49605d944b5 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -30,8 +30,8 @@ namespace marker_utils { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; diff --git a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp index 437e53dbbf1e4..5859bf055d4aa 100644 --- a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp @@ -24,7 +24,7 @@ namespace perception_diagnostics { namespace metrics { -using autoware_universe_utils::toHexString; +using autoware::universe_utils::toHexString; bool isCountObject( const std::uint8_t classification, const std::unordered_map & params) diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp index ca9266ec6247b..d5ac88613fa83 100644 --- a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -31,9 +31,9 @@ double calcLateralDeviation(const std::vector & ref_path, const Pose & tar } const size_t nearest_index = - autoware_motion_utils::findNearestIndex(ref_path, target_pose.position); + autoware::motion_utils::findNearestIndex(ref_path, target_pose.position); return std::abs( - autoware_universe_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); + autoware::universe_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); } double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose) @@ -43,8 +43,8 @@ double calcYawDeviation(const std::vector & ref_path, const Pose & target_ } 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)); + autoware::motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs(autoware::universe_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); } std::vector calcPredictedPathDeviation( @@ -56,9 +56,9 @@ std::vector calcPredictedPathDeviation( return {}; } for (const Pose & p : pred_path.path) { - const size_t nearest_index = autoware_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)); + autoware::universe_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); } return deviations; diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 8732de1fe3393..212af8711a62e 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -23,7 +23,7 @@ namespace perception_diagnostics { -using autoware_universe_utils::inverseTransformPoint; +using autoware::universe_utils::inverseTransformPoint; using object_recognition_utils::convertLabelToString; std::optional MetricsCalculator::calculate(const Metric & metric) const @@ -238,7 +238,7 @@ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( Stat stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = autoware_universe_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -267,7 +267,7 @@ MetricStatMap MetricsCalculator::calcYawDeviationMetrics( Stat stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = autoware_universe_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -326,7 +326,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto stamp = objects.header.stamp; for (const auto & object : objects.objects) { - const auto uuid = autoware_universe_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); const auto predicted_paths = object.kinematics.predicted_paths; for (size_t i = 0; i < predicted_paths.size(); i++) { const auto predicted_path = predicted_paths[i]; @@ -350,7 +350,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto history_pose = history_object.kinematics.initial_pose_with_covariance.pose; const Pose & p = predicted_path.path[j]; const double distance = - autoware_universe_utils::calcDistance2d(p.position, history_pose.position); + autoware::universe_utils::calcDistance2d(p.position, history_pose.position); deviation_map_for_paths[uuid][i].push_back(distance); // Save debug information @@ -424,7 +424,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = autoware_universe_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -444,7 +444,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); // Calculate the absolute difference between current_yaw and previous_yaw const double yaw_diff = - std::abs(autoware_universe_utils::normalizeRadian(current_yaw - previous_yaw)); + std::abs(autoware::universe_utils::normalizeRadian(current_yaw - previous_yaw)); // The yaw difference is flipped if the angle is larger than 90 degrees const double yaw_diff_flip_fixed = std::min(yaw_diff, M_PI - yaw_diff); const double yaw_rate = yaw_diff_flip_fixed / time_diff; @@ -494,7 +494,7 @@ void MetricsCalculator::setPredictedObjects( // store objects to check deviation { - using autoware_universe_utils::toHexString; + using autoware::universe_utils::toHexString; for (const auto & object : objects.objects) { std::string uuid = toHexString(object.object_id); updateObjects(uuid, current_stamp_, object); @@ -557,7 +557,7 @@ void MetricsCalculator::updateHistoryPath() const auto current_pose = object.kinematics.initial_pose_with_covariance.pose; const auto prev_pose = prev_object.kinematics.initial_pose_with_covariance.pose; const auto velocity = - autoware_universe_utils::calcDistance2d(current_pose.position, prev_pose.position) / + autoware::universe_utils::calcDistance2d(current_pose.position, prev_pose.position) / time_diff; if (velocity < parameters_->stopped_velocity_threshold) { continue; @@ -603,9 +603,9 @@ std::vector MetricsCalculator::generateHistoryPathWithPrev( std::vector MetricsCalculator::averageFilterPath( const std::vector & path, const size_t window_size) const { - using autoware_universe_utils::calcAzimuthAngle; - using autoware_universe_utils::calcDistance2d; - using autoware_universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createQuaternionFromYaw; std::vector filtered_path; filtered_path.reserve(path.size()); // Reserve space to avoid reallocations @@ -645,7 +645,7 @@ std::vector MetricsCalculator::averageFilterPath( if (i > 0) { const double azimuth = calcAzimuthAngle(path.at(i - 1).position, path.at(i).position); const double yaw = tf2::getYaw(path.at(i).orientation); - if (autoware_universe_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { + if (autoware::universe_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { continue; } } @@ -662,7 +662,7 @@ std::vector MetricsCalculator::averageFilterPath( const double azimuth_to_prev = calcAzimuthAngle((it - 2)->position, (it - 1)->position); const double azimuth_to_current = calcAzimuthAngle((it - 1)->position, it->position); if ( - std::abs(autoware_universe_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > + std::abs(autoware::universe_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > M_PI_2) { it = filtered_path.erase(it); continue; diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 56b79eb3acf62..63d635af4ffdf 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -156,7 +156,7 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - autoware_universe_utils::appendMarkerArray(added, &marker); + autoware::universe_utils::appendMarkerArray(added, &marker); }; const auto & p = parameters_->debug_marker_parameters; @@ -236,7 +236,7 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParameter( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; @@ -305,8 +305,8 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame void PerceptionOnlineEvaluatorNode::initParameter() { - using autoware_universe_utils::getOrDeclareParameter; - using autoware_universe_utils::updateParam; + using autoware::universe_utils::getOrDeclareParameter; + using autoware::universe_utils::updateParam; auto & p = parameters_; diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index 6085ea0b6ef49..a7a697c4efff6 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -28,12 +28,12 @@ namespace marker_utils { -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; -using autoware_universe_utils::createPoint; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; @@ -46,13 +46,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -167,7 +167,7 @@ std_msgs::msg::ColorRGBA createColorFromString(const std::string & str) const auto r = (hash & 0xFF) / 255.0; const auto g = ((hash >> 8) & 0xFF) / 255.0; const auto b = ((hash >> 16) & 0xFF) / 255.0; - return autoware_universe_utils::createMarkerColor(r, g, b, 0.5); + return autoware::universe_utils::createMarkerColor(r, g, b, 0.5); } MarkerArray createObjectPolygonMarkerArray( @@ -182,7 +182,7 @@ MarkerArray createObjectPolygonMarkerArray( const double z = object.kinematics.initial_pose_with_covariance.pose.position.z; const double height = object.shape.dimensions.z; - const auto polygon = autoware_universe_utils::toPolygon2d( + const auto polygon = autoware::universe_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); for (const auto & p : polygon.outer()) { marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2)); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index a369d36882c9d..e2ab22be2b61b 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -43,7 +43,7 @@ using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification using nav_msgs::msg::Odometry; using TFMessage = tf2_msgs::msg::TFMessage; -using autoware_universe_utils::generateUUID; +using autoware::universe_utils::generateUUID; constexpr double epsilon = 1e-6; diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index f8781ada3d38d..c39969fb3cc5f 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -185,7 +185,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) pose_array_msg.header.frame_id = "map"; for (const Landmark & landmark : landmarks) { const Pose detected_marker_on_map = - autoware_universe_utils::transformPose(landmark.pose, self_pose); + autoware::universe_utils::transformPose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } detected_tag_pose_pub_->publish(pose_array_msg); @@ -194,7 +194,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) // calc new_self_pose const Pose new_self_pose = landmark_manager_.calculate_new_self_pose(landmarks, self_pose, consider_orientation_); - const Pose diff_pose = autoware_universe_utils::inverseTransformPose(new_self_pose, self_pose); + const Pose diff_pose = autoware::universe_utils::inverseTransformPose(new_self_pose, self_pose); const double distance = std::hypot(diff_pose.position.x, diff_pose.position.y, diff_pose.position.z); diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index b2c4a7dd6298a..2fa0fdbdb315d 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -161,7 +161,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // convert base_link to map const Pose detected_landmark_on_map = - autoware_universe_utils::transformPose(detected_landmark_on_base_link, self_pose); + autoware::universe_utils::transformPose(detected_landmark_on_base_link, self_pose); // match to map if (landmarks_map_.count(landmark.id) == 0) { @@ -171,7 +171,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // check all poses for (const Pose mapped_landmark_on_map : landmarks_map_.at(landmark.id)) { // check distance - const double curr_distance = autoware_universe_utils::calcDistance3d( + const double curr_distance = autoware::universe_utils::calcDistance3d( mapped_landmark_on_map.position, detected_landmark_on_map.position); if (curr_distance > min_distance) { continue; diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index ce1d7a95178ee..65003f5fe9864 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -142,7 +142,7 @@ class EKFLocalizer : public rclcpp::Node std::shared_ptr tf_br_; //!< @brief logger configure module - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; //!< @brief extended kalman filter instance. std::unique_ptr ekf_module_; @@ -236,7 +236,7 @@ class EKFLocalizer : public rclcpp::Node const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); - autoware_universe_utils::StopWatch stop_watch_; + autoware::universe_utils::StopWatch stop_watch_; friend class EKFLocalizerTestSuite; // for test code }; diff --git a/localization/ekf_localizer/src/covariance.cpp b/localization/ekf_localizer/src/covariance.cpp index 4e68de981e196..4655ea8a89855 100644 --- a/localization/ekf_localizer/src/covariance.cpp +++ b/localization/ekf_localizer/src/covariance.cpp @@ -17,7 +17,7 @@ #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "ekf_localizer/state_index.hpp" -using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) { diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5400c0a70b6f3..11fa2b6713a52 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -96,7 +96,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) std::shared_ptr(this, [](auto) {})); ekf_module_ = std::make_unique(warning_, params_); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); z_filter_.set_proc_dev(params_.z_filter_proc_dev); roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); @@ -264,7 +264,7 @@ void EKFLocalizer::timer_tf_callback() const rclcpp::Time current_time = this->now(); geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = autoware_universe_utils::pose2transform( + transform_stamped = autoware::universe_utils::pose2transform( ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link"); transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); @@ -436,9 +436,9 @@ void EKFLocalizer::update_simple_1d_filters( { double z = pose.pose.pose.position.z; - const auto rpy = autoware_universe_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); double pitch_dev = @@ -454,9 +454,9 @@ void EKFLocalizer::init_simple_1d_filters( { double z = pose.pose.pose.position.z; - const auto rpy = autoware_universe_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 82e01f6b81065..ba6b7dedca82c 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -65,7 +65,7 @@ void EKFModule::initialize( x(IDX::VX) = 0.0; x(IDX::WZ) = 0.0; - using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; @@ -97,13 +97,13 @@ geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; - current_ekf_pose.pose.position = autoware_universe_utils::createPoint(x, y, z); + current_ekf_pose.pose.position = autoware::universe_utils::createPoint(x, y, z); if (get_biased_yaw) { current_ekf_pose.pose.orientation = - autoware_universe_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); + autoware::universe_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); } else { current_ekf_pose.pose.orientation = - autoware_universe_utils::createQuaternionFromRPY(roll, pitch, yaw); + autoware::universe_utils::createQuaternionFromRPY(roll, pitch, yaw); } return current_ekf_pose; } @@ -285,7 +285,7 @@ bool EKFModule::measurement_update_pose( geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time) { - const auto rpy = autoware_universe_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); PoseWithCovariance pose_with_z_delay; pose_with_z_delay = pose; diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index fbc177d602ff0..faf8768938b7d 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -38,7 +38,7 @@ Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix3d r; - using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); @@ -49,7 +49,7 @@ Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix2d r; - using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_YAW); return r * static_cast(smoothing_step); diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index eb1db06b72998..f1f7214c3b848 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -43,7 +43,7 @@ namespace autoware::gyro_odometer class GyroOdometerNode : public rclcpp::Node { private: - using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); @@ -67,8 +67,8 @@ class GyroOdometerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; - std::shared_ptr transform_listener_; - std::unique_ptr logger_configure_; + std::shared_ptr transform_listener_; + std::unique_ptr logger_configure_; std::string output_frame_; double message_timeout_sec_; diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 0878c44ffa2cb..c661f63d91535 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -33,7 +33,7 @@ namespace autoware::gyro_odometer std::array transform_covariance(const std::array & cov) { - using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); @@ -52,8 +52,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_arrived_(false), imu_arrived_(false) { - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, @@ -208,8 +208,8 @@ void GyroOdometerNode::concat_gyro_and_odometer() gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } - using COV_IDX_XYZ = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX_XYZ = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // calc mean, covariance double vx_mean = 0; diff --git a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 12f0a1141d199..a178a305f6d12 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -43,7 +43,7 @@ class LocalizationErrorMonitor : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; double scale_; double error_ellipse_size_; diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp index 51fc1607d048a..204afa7bdec2f 100644 --- a/localization/localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -55,7 +55,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o diag_pub_ = this->create_publisher("/diagnostics", 10); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 5a28ed7eb8a5e..2f3eb82b1c217 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -203,7 +203,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr diagnostics_ndt_align_; std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; HyperParameters param_; }; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index f392ba4b3c19f..3bf6e355000bf 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -210,7 +210,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) diagnostics_trigger_node_ = std::make_unique(this, "trigger_node_service_status"); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void NDTScanMatcher::callback_timer() @@ -615,7 +615,7 @@ bool NDTScanMatcher::callback_sensor_points_main( pcl::shared_ptr> sensor_points_in_map_ptr( new pcl::PointCloud); - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); @@ -671,10 +671,10 @@ void NDTScanMatcher::transform_sensor_measurement( } const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - autoware_universe_utils::transform2pose(transform); + autoware::universe_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); } @@ -686,7 +686,7 @@ void NDTScanMatcher::publish_tf( result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - autoware_universe_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); + autoware::universe_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -730,7 +730,7 @@ void NDTScanMatcher::publish_marker( marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = autoware_universe_utils::createMarkerScale(0.3, 0.1, 0.1); + marker.scale = autoware::universe_utils::createMarkerScale(0.3, 0.1, 0.1); int i = 0; marker.ns = "result_pose_matrix_array"; marker.action = visualization_msgs::msg::Marker::ADD; @@ -759,7 +759,7 @@ void NDTScanMatcher::publish_initial_to_result( { geometry_msgs::msg::PoseStamped initial_to_result_relative_pose_stamped; initial_to_result_relative_pose_stamped.pose = - autoware_universe_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); + autoware::universe_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); @@ -1091,7 +1091,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); auto sensor_points_in_map_ptr = std::make_shared>(); - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud( initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp index 0866234383727..3a565e7f2e4df 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp @@ -28,9 +28,9 @@ PcdMapBasedRule::PcdMapBasedRule( shared_data_(std::move(shared_data)) { const int pcd_density_upper_threshold = - autoware_universe_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); const int pcd_density_lower_threshold = - autoware_universe_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); pcd_occupancy_ = std::make_unique( pcd_density_upper_threshold, pcd_density_lower_threshold); diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp index 4228ade6be701..013b4b38f9ef6 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -53,7 +53,7 @@ class PoseEstimatorArbiter : public rclcpp::Node // Set of running pose estimators specified by ros param `pose_sources` const std::unordered_set running_estimator_list_; // Configuration to allow changing the log level by service - const std::unique_ptr logger_configure_; + const std::unique_ptr logger_configure_; // This is passed to several modules (stoppers & rule) so that all modules can access common data // without passing them as arguments. Also, modules can register subscriber callbacks through diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp index 4d655875ba325..8e25628d6e0fc 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -46,7 +46,7 @@ PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_estimator_arbiter", options), running_estimator_list_(parse_estimator_name_args( declare_parameter>("pose_sources"), get_logger())), - logger_configure_(std::make_unique(this)) + logger_configure_(std::make_unique(this)) { // Shared data shared_data_ = std::make_shared(); diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 1e351f8a90d7b..7d75a1fc80f2e 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -56,7 +56,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) stop_check_duration_ = declare_parameter("stop_check_duration"); stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index 3a0896a07ba98..a813ec6459810 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -55,7 +55,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr stop_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); 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 140fb3079de45..31f04029bd00e 100644 --- a/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp @@ -21,7 +21,7 @@ #include #include -class StopCheckModule : public autoware_motion_utils::VehicleStopCheckerBase +class StopCheckModule : public autoware::motion_utils::VehicleStopCheckerBase { public: StopCheckModule(rclcpp::Node * node, double buffer_duration); diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index 436fd4eee7692..28398588809eb 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -123,7 +123,7 @@ void PoseInstabilityDetector::callback_timer() // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_dr = autoware_universe_utils::inverseTransformPose(*dr_pose, latest_ekf_pose); + const Pose ekf_to_dr = autoware::universe_utils::inverseTransformPose(*dr_pose, latest_ekf_pose); const geometry_msgs::msg::Point pos = ekf_to_dr.position; const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_dr.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index aa0244e5d7e90..3ea23ded92efa 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -198,7 +198,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = autoware_universe_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = autoware::universe_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -228,7 +228,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = autoware_universe_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = autoware::universe_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index f319fcc9fd77a..9539e44f61276 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -98,7 +98,7 @@ void GraphSegment::on_image(const Image & msg) cv::resize(image, resized, cv::Size(), 0.5, 0.5); // Execute graph-based segmentation - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; cv::Mat segmented; segmentation_->processImage(resized, segmented); RCLCPP_INFO_STREAM(get_logger(), "segmentation time: " << stop_watch.toc() * 1000 << "[ms]"); diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index f0f451834c66e..c5cec31e24844 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -53,7 +53,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st cv::Mat lines; { - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; line_segment_detector_->detect(gray_image, lines); if (lines.size().width != 0) { line_segment_detector_->drawSegments(gray_image, lines); diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index ad38c320fc510..5590387e5ba26 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -147,7 +147,7 @@ class UndistortNode : public rclcpp::Node sub_compressed_image_.reset(); } - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } @@ -161,7 +161,7 @@ class UndistortNode : public rclcpp::Node make_remap_lut(); } - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp index 35113b4c5af36..c894cd999fb14 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp @@ -130,7 +130,7 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments_msg) { - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; const rclcpp::Time stamp = line_segments_msg.header.stamp; std::optional opt_array = this->get_synchronized_particle_array(stamp); if (!opt_array.has_value()) { @@ -258,7 +258,7 @@ float abs_cos(const Eigen::Vector3f & t, float deg) { const float radian = deg * M_PI / 180.0; Eigen::Vector2f x(t.x(), t.y()); - Eigen::Vector2f y(autoware_universe_utils::cos(radian), autoware_universe_utils::sin(radian)); + Eigen::Vector2f y(autoware::universe_utils::cos(radian), autoware::universe_utils::sin(radian)); x.normalize(); return std::abs(x.dot(y)); } diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index 69f3642c8eff6..6255236f2802e 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -169,7 +169,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = autoware_universe_utils::createMarkerColor(0, 0, 1.0f, 1.0f); + marker.color = autoware::universe_utils::createMarkerColor(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp index 452805803ffa6..8efb90cc87c89 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -39,10 +39,10 @@ namespace autoware::crosswalk_traffic_light_estimator { +using autoware::universe_utils::DebugPublisher; +using autoware::universe_utils::StopWatch; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; -using autoware_universe_utils::DebugPublisher; -using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 86633721c936b..ce1c587cc3b01 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -116,6 +116,7 @@ struct PredictionTimeHorizon using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; +using autoware::universe_utils::StopWatch; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -129,7 +130,6 @@ using autoware_perception_msgs::msg::TrafficLightElement; using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node @@ -143,12 +143,12 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_markers_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_traffic_signals_{this, "/traffic_signals"}; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; // Object History std::unordered_map> road_users_history; @@ -169,7 +169,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::vector & parameters); // Pose Transform Listener - autoware_universe_utils::TransformListener transform_listener_{this}; + autoware::universe_utils::TransformListener transform_listener_{this}; // Path Generator std::shared_ptr path_generator_; @@ -221,7 +221,7 @@ class MapBasedPredictionNode : public rclcpp::Node bool match_lost_and_appeared_crosswalk_users_; bool remember_lost_crosswalk_users_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // Member Functions void mapCallback(const LaneletMapBin::ConstSharedPtr msg); @@ -314,8 +314,8 @@ class MapBasedPredictionNode : public rclcpp::Node inline std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using autoware_universe_utils::calcCurvature; - using autoware_universe_utils::getPoint; + using autoware::universe_utils::calcCurvature; + using autoware::universe_utils::getPoint; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); diff --git a/perception/autoware_map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp index 638c10c1a3b1a..bd40ee0981c0e 100644 --- a/perception/autoware_map_based_prediction/src/debug.cpp +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -29,7 +29,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( marker.type = visualization_msgs::msg::Marker::CUBE; marker.action = visualization_msgs::msg::Marker::ADD; marker.pose = object.kinematics.pose_with_covariance.pose; - marker.scale = autoware_universe_utils::createMarkerScale(3.0, 1.0, 1.0); + marker.scale = autoware::universe_utils::createMarkerScale(3.0, 1.0, 1.0); // Color by maneuver double r = 0.0; @@ -42,7 +42,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( } else { b = 1.0; } - marker.color = autoware_universe_utils::createMarkerColor(r, g, b, 0.8); + marker.color = autoware::universe_utils::createMarkerColor(r, g, b, 0.8); return marker; } 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 0916a4568561f..f505cb7cc5592 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 @@ -92,10 +92,10 @@ double calcAbsLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = autoware_universe_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware::universe_utils::createPoint(x, y, 0.0); } - return std::fabs(autoware_motion_utils::calcLateralOffset(boundary_path, search_pose.position)); + return std::fabs(autoware::motion_utils::calcLateralOffset(boundary_path, search_pose.position)); } /** @@ -218,7 +218,7 @@ double calcAbsYawDiffBetweenLaneletAndObject( const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); return abs_norm_delta; } @@ -418,7 +418,7 @@ boost::optional isReachableCrosswalkEdgePoints( const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = autoware_universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { @@ -529,7 +529,7 @@ bool hasPotentialToReach( { const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = autoware_universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; constexpr double stop_velocity_th = 0.14; // [m/s] const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); @@ -547,16 +547,16 @@ bool hasPotentialToReach( const double pedestrian_to_crosswalk_left_direction = std::atan2(left_point.y() - obj_pos.y, left_point.x() - obj_pos.x); return std::make_pair( - autoware_universe_utils::normalizeRadian( + autoware::universe_utils::normalizeRadian( pedestrian_to_crosswalk_right_direction - pedestrian_to_crosswalk_center_direction), - autoware_universe_utils::normalizeRadian( + autoware::universe_utils::normalizeRadian( pedestrian_to_crosswalk_left_direction - pedestrian_to_crosswalk_center_direction)); }(); const double pedestrian_heading_rel_direction = [&]() { const double pedestrian_heading_direction = std::atan2(obj_vel.x * std::sin(yaw), obj_vel.x * std::cos(yaw)); - return autoware_universe_utils::normalizeRadian( + return autoware::universe_utils::normalizeRadian( pedestrian_heading_direction - pedestrian_to_crosswalk_center_direction); }(); @@ -658,7 +658,7 @@ ObjectClassification::_label_type changeLabelForPrediction( if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; constexpr float high_speed_threshold = - autoware_universe_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + autoware::universe_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -673,7 +673,7 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::PEDESTRIAN: { const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float max_velocity_for_human_mps = - autoware_universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + autoware::universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); @@ -859,15 +859,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); processing_time_publisher_ = - std::make_unique(this, "map_based_prediction"); + std::make_unique(this, "map_based_prediction"); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -875,7 +875,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "max_lateral_accel", max_lateral_accel_); updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); @@ -1010,14 +1010,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt if ( label_for_prediction == ObjectClassification::PEDESTRIAN || label_for_prediction == ObjectClassification::BICYCLE) { - const std::string object_id = autoware_universe_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); current_crosswalk_users.emplace(object_id, object); } } std::unordered_set predicted_crosswalk_users_ids; for (const auto & object : in_objects->objects) { - std::string object_id = autoware_universe_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); TrackedObject transformed_object = object; // transform object frame if it's based on map frame @@ -1036,7 +1036,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt switch (label) { case ObjectClassification::PEDESTRIAN: case ObjectClassification::BICYCLE: { - std::string object_id = autoware_universe_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (match_lost_and_appeared_crosswalk_users_) { object_id = tryMatchNewObjectToDisappeared(object_id, current_crosswalk_users); } @@ -1339,11 +1339,11 @@ bool MapBasedPredictionNode::isIntersecting( const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) { - const auto p1 = autoware_universe_utils::createPoint(point1.x, point1.y, 0.0); - const auto p2 = autoware_universe_utils::createPoint(point2.x, point2.y, 0.0); - const auto p3 = autoware_universe_utils::createPoint(point3.x(), point3.y(), 0.0); - const auto p4 = autoware_universe_utils::createPoint(point4.x(), point4.y(), 0.0); - const auto intersection = autoware_universe_utils::intersect(p1, p2, p3, p4); + const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0); + const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0); + const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0); + const auto p4 = autoware::universe_utils::createPoint(point4.x(), point4.y(), 0.0); + const auto intersection = autoware::universe_utils::intersect(p1, p2, p3, p4); return intersection.has_value(); } @@ -1491,7 +1491,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) // Compute yaw angle from the velocity and position of the object const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const auto future_object_pose = autoware_universe_utils::calcOffsetPose( + const auto future_object_pose = autoware::universe_utils::calcOffsetPose( object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); // assumption: the object vx is much larger than vy @@ -1510,16 +1510,16 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle object.kinematics.pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw( - autoware_universe_utils::pi + original_yaw); + autoware::universe_utils::createQuaternionFromYaw( + autoware::universe_utils::pi + original_yaw); break; } default: { - const auto updated_object_yaw = autoware_universe_utils::calcAzimuthAngle( + const auto updated_object_yaw = autoware::universe_utils::calcAzimuthAngle( object_pose.position, future_object_pose.position); object.kinematics.pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(updated_object_yaw); + autoware::universe_utils::createQuaternionFromYaw(updated_object_yaw); break; } } @@ -1536,7 +1536,7 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( const bool isDisappeared = std::none_of( in_objects->objects.begin(), in_objects->objects.end(), [&it](autoware_perception_msgs::msg::TrackedObject obj) { - return autoware_universe_utils::toHexString(obj.object_id) == it->first.first; + return autoware::universe_utils::toHexString(obj.object_id) == it->first.first; }); if (isDisappeared) { it = stopped_times_against_green_.erase(it); @@ -1645,7 +1645,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets - const std::string object_id = autoware_universe_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) != 0) { const std::vector & possible_lanelet = road_users_history.at(object_id).back().future_possible_lanelets; @@ -1663,7 +1663,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet.second, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); // Step3. Check if the closest lanelet is valid, and add all @@ -1690,7 +1690,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); const double delta_yaw = obj_yaw - lane_yaw; - const double abs_norm_delta_yaw = std::fabs(autoware_universe_utils::normalizeRadian(delta_yaw)); + const double abs_norm_delta_yaw = std::fabs(autoware::universe_utils::normalizeRadian(delta_yaw)); // compute lateral distance const auto centerline = current_lanelet.centerline(); @@ -1700,7 +1700,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( converted_centerline.push_back(converted_p); } const double lat_dist = - std::fabs(autoware_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 @@ -1719,7 +1719,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { - std::string object_id = autoware_universe_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); ObjectData single_object_data; @@ -1729,7 +1729,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( single_object_data.pose = object.kinematics.pose_with_covariance.pose; const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); single_object_data.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(object_yaw); + autoware::universe_utils::createQuaternionFromYaw(object_yaw); single_object_data.time_delay = std::fabs((this->get_clock()->now() - header.stamp).seconds()); single_object_data.twist = object.kinematics.twist_with_covariance.twist; @@ -1933,7 +1933,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( throw std::logic_error("Lane change detection method is invalid."); }(); - const std::string object_id = autoware_universe_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return current_maneuver; } @@ -1972,7 +1972,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( const double /*object_detected_time*/) { // Step1. Check if we have the object in the buffer - const std::string object_id = autoware_universe_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -2043,7 +2043,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( const double /*object_detected_time*/) { // Step1. Check if we have the object in the buffer - const std::string object_id = autoware_universe_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -2081,7 +2081,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( for (const auto & lanelet : prev_lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.position); const double delta_yaw = tf2::getYaw(prev_pose.orientation) - lane_yaw; - const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); if (normalized_delta_yaw < closest_prev_yaw) { closest_prev_yaw = normalized_delta_yaw; prev_lanelet = lanelet; @@ -2091,7 +2091,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( // Step4. Check if the vehicle has changed lane const auto current_lanelet = current_lanelet_data.lanelet; const auto current_pose = object.kinematics.pose_with_covariance.pose; - const double dist = autoware_universe_utils::calcDistance2d(prev_pose, current_pose); + const double dist = autoware::universe_utils::calcDistance2d(prev_pose, current_pose); lanelet::routing::LaneletPaths possible_paths = routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); bool has_lane_changed = true; @@ -2176,8 +2176,8 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( const double curr_yaw = prev_yaw + wz * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, curr_z); - current_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); + current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, curr_z); + current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); return current_pose; } @@ -2189,10 +2189,10 @@ double MapBasedPredictionNode::calcRightLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = autoware_universe_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware::universe_utils::createPoint(x, y, 0.0); } - return std::fabs(autoware_motion_utils::calcLateralOffset(boundary_path, search_pose.position)); + return std::fabs(autoware::motion_utils::calcLateralOffset(boundary_path, search_pose.position)); } double MapBasedPredictionNode::calcLeftLateralOffset( @@ -2204,7 +2204,7 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { - std::string object_id = autoware_universe_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return; } @@ -2331,7 +2331,7 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); + current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } @@ -2353,7 +2353,7 @@ std::vector MapBasedPredictionNode::convertPathType( // Prevent from inserting same points if (!converted_path.empty()) { const auto last_p = converted_path.back(); - const double tmp_dist = autoware_universe_utils::calcDistance2d(last_p, current_p); + const double tmp_dist = autoware::universe_utils::calcDistance2d(last_p, current_p); if (tmp_dist < 1e-6) { prev_p = current_p; continue; @@ -2362,7 +2362,7 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); + current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } @@ -2370,7 +2370,7 @@ std::vector MapBasedPredictionNode::convertPathType( // Resample Path const auto resampled_converted_path = - autoware_motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); + autoware::motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); converted_paths.push_back(resampled_converted_path); } @@ -2402,7 +2402,7 @@ bool MapBasedPredictionNode::isDuplicated( for (const auto & prev_predicted_path : predicted_paths) { const auto prev_path_end = prev_predicted_path.path.back().position; const auto current_path_end = predicted_path.path.back().position; - const double dist = autoware_universe_utils::calcDistance2d(prev_path_end, current_path_end); + const double dist = autoware::universe_utils::calcDistance2d(prev_path_end, current_path_end); if (dist < CLOSE_PATH_THRESHOLD) { return true; } @@ -2452,10 +2452,10 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( }(); const auto key = - std::make_pair(autoware_universe_utils::toHexString(object.object_id), signal_id); + std::make_pair(autoware::universe_utils::toHexString(object.object_id), signal_id); if ( signal_color == TrafficLightElement::GREEN && - autoware_universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < threshold_velocity_assumed_as_stopping_) { stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index f28aae2ea9794..aeb1b0fedd33f 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -50,7 +50,7 @@ PredictedPath PathGenerator::generatePathToTargetPoint( const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); const auto pedestrian_to_entry_point_orientation = - autoware_universe_utils::createQuaternionFromYaw(std::atan2( + autoware::universe_utils::createQuaternionFromYaw(std::atan2( pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); for (double dt = 0.0; dt < arrival_time + ep; dt += sampling_time_interval_) { @@ -94,10 +94,10 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); const auto pedestrian_to_entry_point_orientation = - autoware_universe_utils::createQuaternionFromYaw(std::atan2( + autoware::universe_utils::createQuaternionFromYaw(std::atan2( pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); const auto entry_to_exit_point_normalized = entry_to_exit_point.normalized(); - const auto entry_to_exit_point_orientation = autoware_universe_utils::createQuaternionFromYaw( + const auto entry_to_exit_point_orientation = autoware::universe_utils::createQuaternionFromYaw( std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x())); for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { @@ -174,7 +174,7 @@ PredictedPath PathGenerator::generateStraightPath( path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); path.path.reserve(static_cast((duration) / sampling_time_interval_)); for (double dt = 0.0; dt < duration; dt += sampling_time_interval_) { - const auto future_obj_pose = autoware_universe_utils::calcOffsetPose( + const auto future_obj_pose = autoware::universe_utils::calcOffsetPose( object_pose, object_twist.linear.x * dt, object_twist.linear.y * dt, 0.0); path.path.push_back(future_obj_pose); } @@ -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 = autoware_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 @@ -319,7 +319,7 @@ PosePath PathGenerator::interpolateReferencePath( base_path_y.at(i) = base_path.at(i).position.y; base_path_z.at(i) = base_path.at(i).position.z; if (i > 0) { - base_path_s.at(i) = base_path_s.at(i - 1) + autoware_universe_utils::calcDistance2d( + base_path_s.at(i) = base_path_s.at(i - 1) + autoware::universe_utils::calcDistance2d( base_path.at(i - 1), base_path.at(i)); } } @@ -344,16 +344,16 @@ PosePath PathGenerator::interpolateReferencePath( for (size_t i = 0; i < interpolate_num - 1; ++i) { geometry_msgs::msg::Pose interpolated_pose; const auto current_point = - autoware_universe_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); - const auto next_point = autoware_universe_utils::createPoint( + autoware::universe_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); + const auto next_point = autoware::universe_utils::createPoint( spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); - const double yaw = autoware_universe_utils::calcAzimuthAngle(current_point, next_point); - interpolated_pose.position = autoware_universe_utils::createPoint( + const double yaw = autoware::universe_utils::calcAzimuthAngle(current_point, next_point); + interpolated_pose.position = autoware::universe_utils::createPoint( spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); - interpolated_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + interpolated_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); interpolated_path.at(i) = interpolated_pose; } - interpolated_path.back().position = autoware_universe_utils::createPoint( + interpolated_path.back().position = autoware::universe_utils::createPoint( spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; @@ -376,14 +376,14 @@ PredictedPath PathGenerator::convertToPredictedPath( // Converted Pose auto predicted_pose = - autoware_universe_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); + autoware::universe_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); predicted_pose.position.z = object.kinematics.pose_with_covariance.pose.position.z; if (i == 0) { predicted_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; } else { - const double yaw = autoware_universe_utils::calcAzimuthAngle( + const double yaw = autoware::universe_utils::calcAzimuthAngle( predicted_path.path.at(i - 1).position, predicted_pose.position); - predicted_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + predicted_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); } predicted_path.path.at(i) = predicted_pose; } @@ -399,8 +399,8 @@ FrenetPoint PathGenerator::getFrenetPoint( const auto obj_point = object.kinematics.pose_with_covariance.pose.position; const size_t nearest_segment_idx = - autoware_motion_utils::findNearestSegmentIndex(ref_path, obj_point); - const double l = autoware_motion_utils::calcLongitudinalOffsetToSegment( + 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); @@ -480,8 +480,9 @@ 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 = 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 = + 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/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp index adc02680e0d94..a60a76f1c2ef0 100644 --- a/perception/cluster_merger/include/cluster_merger/node.hpp +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -59,7 +59,7 @@ class ClusterMergerNode : public rclcpp::Node std::string output_frame_id_; std::vector::SharedPtr> sub_objects_array{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; void objectsCallback( const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index f72e87e2e3610..2962ba1e28a75 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -109,8 +109,8 @@ DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distance_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 6e8f527cc465c..e14ae7d55a1d4 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -72,8 +72,8 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_approximate_compare_map_filter"); diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 720b5cf1d229a..449ae76181bc6 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -31,8 +31,8 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 3aacf31c9332f..0a660bfffd7fb 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -101,8 +101,8 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_distance_based_compare_map_filter"); diff --git a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp index 3a29e7dd1d533..02e4c9c3ca29a 100644 --- a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp +++ b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp @@ -36,7 +36,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input); void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs); }; diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp index 43a1d4d3d26ea..3fe9e6fde8683 100644 --- a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp @@ -24,7 +24,7 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt sub_ = this->create_subscription( "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1)); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void DetectedObjectFeatureRemover::objectCallback( diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index b96a57b12a233..9b9894d56a424 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -35,10 +35,10 @@ namespace object_lanelet_filter { -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::MultiPoint2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; class ObjectLaneletFilterNode : public rclcpp::Node { @@ -52,7 +52,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; - std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; @@ -89,7 +89,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node const autoware_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_lanelet_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index d9eb3d367d4cf..ea70d62fd952d 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -54,7 +54,7 @@ class ObjectPositionFilterNode : public rclcpp::Node utils::FilterTargetLabel filter_target_; bool isObjectInBounds(const autoware_perception_msgs::msg::DetectedObject & object) const; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_position_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 450072f258e2a..47685dec7dbdc 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -131,7 +131,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr objects_pub_; message_filters::Subscriber objects_sub_; message_filters::Subscriber obstacle_pointcloud_sub_; - std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr debug_publisher_{nullptr}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -145,7 +145,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; std::unique_ptr validator_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; private: void onObjectsAndObstaclePointCloud( diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index 399db8cd8c1f3..beb8faf5db1a3 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -43,7 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node message_filters::Subscriber occ_grid_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; typedef message_filters::sync_policies::ApproximateTime< autoware_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid> diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 42247a2e36d2b..cc3a600783a8f 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -63,9 +63,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "output/object", rclcpp::QoS{1}); debug_publisher_ = - std::make_unique(this, "object_lanelet_filter"); + std::make_unique(this, "object_lanelet_filter"); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void ObjectLaneletFilterNode::mapCallback( @@ -237,7 +237,8 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( const auto footprint = setFootprint(object); for (const auto & point : footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - autoware_universe_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + autoware::universe_utils::transformPoint( + point, object.kinematics.pose_with_covariance.pose); polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); @@ -246,7 +247,8 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( // if object do not have bounding box, check each footprint is inside polygon for (const auto & point : object.shape.footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - autoware_universe_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + autoware::universe_utils::transformPoint( + point, object.kinematics.pose_with_covariance.pose); geometry_msgs::msg::Pose point2d; point2d.position.x = point_transformed.x; point2d.position.y = point_transformed.y; @@ -296,7 +298,7 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_velocity_yaw - lane_yaw; - const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 8e22cd1b88d88..dccff8a6ccc67 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -43,7 +43,7 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void ObjectPositionFilterNode::objectCallback( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 10e923b91c1bd..fbb3f863be7c4 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -33,7 +33,7 @@ namespace obstacle_pointcloud_based_validator { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = autoware_universe_utils::Polygon2d; +using Polygon2d = autoware::universe_utils::Polygon2d; Validator::Validator(PointsNumThresholdParam & points_num_threshold_param) { @@ -96,8 +96,8 @@ std::optional Validator2D::getPointCloudWithinObject( { std::vector vertices_array; pcl::Vertices vertices; - Polygon2d poly2d = - autoware_universe_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = autoware::universe_utils::toPolygon2d( + object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -211,8 +211,8 @@ std::optional Validator3D::getPointCloudWithinObject( auto const object_height = object.shape.dimensions.x; auto z_min = object_position.z - object_height / 2.0f; auto z_max = object_position.z + object_height / 2.0f; - Polygon2d poly2d = - autoware_universe_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = autoware::universe_utils::toPolygon2d( + object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -304,13 +304,13 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - debug_publisher_ = std::make_unique( + debug_publisher_ = std::make_unique( this, "obstacle_pointcloud_based_validator"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index ec29818ee6491..b8682a6e56b3b 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -33,7 +33,7 @@ namespace occupancy_grid_based_validator { using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = autoware_universe_utils::Polygon2d; +using Polygon2d = autoware::universe_utils::Polygon2d; OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options) : rclcpp::Node("occupancy_grid_based_validator", node_options), @@ -53,7 +53,7 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -107,8 +107,8 @@ std::optional OccupancyGridBasedValidator::getMask( const auto & resolution = occupancy_grid.info.resolution; const auto & origin = occupancy_grid.info.origin; std::vector pixel_vertices; - Polygon2d poly2d = - autoware_universe_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = autoware::universe_utils::toPolygon2d( + object.kinematics.pose_with_covariance.pose, object.shape); bool is_polygon_within_image = true; for (const auto & p : poly2d.outer()) { diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index 40caa06d3acbf..0169082a2253a 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -60,9 +60,9 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); processing_time_publisher_ = - std::make_unique(node, "detection_by_tracker"); + std::make_unique(node, "detection_by_tracker"); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); this->startStopWatch(); } @@ -103,8 +103,8 @@ class Debugger rclcpp::Publisher::SharedPtr merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; autoware_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index de9453c68ca95..d6d030aadb000 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -84,7 +84,7 @@ class DetectionByTracker : public rclcpp::Node detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index dd63013bff038..4e502b359120d 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -61,7 +61,7 @@ boost::optional getReferenceYawInfo(const uint8_t label, const const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceYawInfo{yaw, autoware_universe_utils::deg2rad(30)}; + return ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; } else { return boost::none; } @@ -136,9 +136,9 @@ bool TrackerHandler::estimateTrackedObjects( estimated_object.kinematics.pose_with_covariance.pose.position.y = y + vx * std::sin(yaw) * dt.seconds(); estimated_object.kinematics.pose_with_covariance.pose.position.z = z; - const float yaw_hat = autoware_universe_utils::normalizeRadian(yaw + wz * dt.seconds()); + const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); estimated_object.kinematics.pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw_hat); + autoware::universe_utils::createQuaternionFromYaw(yaw_hat); output.objects.push_back(estimated_object); } return true; @@ -178,7 +178,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void DetectionByTracker::setMaxSearchRange() @@ -284,7 +284,7 @@ void DetectionByTracker::divideUnderSegmentedObjects( for (const auto & initial_object : in_cluster_objects.feature_objects) { // search near object - const float distance = autoware_universe_utils::calcDistance2d( + const float distance = autoware::universe_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); if (max_search_range < distance) { @@ -420,7 +420,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( pcl::PointCloud pcl_merged_cluster; for (const auto & initial_object : in_cluster_objects.feature_objects) { - const float distance = autoware_universe_utils::calcDistance2d( + const float distance = autoware::universe_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 1d3d091165b4c..3b249e45ae194 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -356,7 +356,7 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) // Convert elevation layer to OpenCV image to fill in holes. // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). namespace bg = boost::geometry; - using autoware_universe_utils::Point2d; + using autoware::universe_utils::Point2d; elevation_map_.add("inpaint_mask", 0.0); diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index 31f24766de9a9..f986b0334935f 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -39,9 +39,9 @@ EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); debug_publisher_ = - std::make_unique(this, "euclidean_cluster"); + std::make_unique(this, "euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 23724ae880827..92f10696d17dc 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -41,8 +41,8 @@ class EuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 2160fd50faafb..e9425095a3b0d 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -43,8 +43,8 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); stop_watch_ptr_ = - std::make_unique>(); - debug_publisher_ = std::make_unique( + std::make_unique>(); + debug_publisher_ = std::make_unique( this, "voxel_grid_based_euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index 3a954436ce8ca..b0c5d0e5a7fbf 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -41,8 +41,8 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 39498f9c8c82f..3ae41173a7a7d 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -280,9 +280,9 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult onParameter(const std::vector & p); // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 7af02da734524..7853aa400b6a5 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -25,11 +25,11 @@ namespace ground_segmentation { +using autoware::universe_utils::calcDistance3d; +using autoware::universe_utils::deg2rad; +using autoware::universe_utils::normalizeDegree; +using autoware::universe_utils::normalizeRadian; using autoware::vehicle_info_utils::VehicleInfoUtils; -using autoware_universe_utils::calcDistance3d; -using autoware_universe_utils::deg2rad; -using autoware_universe_utils::normalizeDegree; -using autoware_universe_utils::normalizeRadian; using pointcloud_preprocessor::get_param; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) @@ -79,8 +79,8 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "scan_ground_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 92859a80e48f6..df8bf66433300 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -138,8 +138,8 @@ class FusionNode : public rclcpp::Node float filter_scope_max_z_; /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index e561642b76951..20273fd1de547 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -130,8 +130,8 @@ FusionNode::FusionNode( // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index fb99364147509..2e997f3e8e60f 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -70,7 +70,7 @@ void closest_cluster( std::memcpy(&point.y, &cluster.data[i * point_step + y_offset], sizeof(float)); std::memcpy(&point.z, &cluster.data[i * point_step + z_offset], sizeof(float)); - point_data.distance = autoware_universe_utils::calcDistance2d(center, point); + point_data.distance = autoware::universe_utils::calcDistance2d(center, point); point_data.orig_index = i; points_data.push_back(point_data); } @@ -258,7 +258,7 @@ pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); for (std::size_t i = 0; i < cluster.points.size(); ++i) { pcl::PointXYZ point = cluster.points.at(i); - double dist_closest_point = autoware_universe_utils::calcDistance2d(point, orig_point); + double dist_closest_point = autoware::universe_utils::calcDistance2d(point, orig_point); if (min_dist > dist_closest_point) { min_dist = dist_closest_point; closest_point = pcl::PointXYZ(point.x, point.y, point.z); diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp index 9082e209fb330..f159f125677c4 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp @@ -47,8 +47,8 @@ class LidarInstanceSegmentationNode : public rclcpp::Node std::shared_ptr detector_ptr_; std::shared_ptr debugger_ptr_; void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; public: explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index 592c63ed22169..b704bab39338a 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -88,7 +88,8 @@ bool LidarApolloInstanceSegmentation::transformCloud( target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500)); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - autoware_universe_utils::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix); + autoware::universe_utils::transformPointCloud( + pcl_input, pcl_transformed_cloud, affine_matrix); transformed_cloud.header.frame_id = target_frame_; pcl_transformed_cloud.header.frame_id = target_frame_; } catch (tf2::TransformException & ex) { @@ -103,7 +104,7 @@ bool LidarApolloInstanceSegmentation::transformCloud( pcl::PointCloud pointcloud_with_z_offset; Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform); pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud); diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/lidar_apollo_instance_segmentation/src/node.cpp index 6034fe917bf42..93b2f3344de99 100644 --- a/perception/lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/node.cpp @@ -28,8 +28,8 @@ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( using std::placeholders::_1; // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "lidar_apollo_instance_segmentation"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index a516d35edfd6b..e78cd57b3a4a1 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -147,7 +147,7 @@ void ApolloLidarSegmentation::transformCloud( tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( in_cluster, transformed_cloud_cluster, affine_matrix); transformed_cloud_cluster.header.frame_id = target_frame_; } else { @@ -158,7 +158,7 @@ void ApolloLidarSegmentation::transformCloud( if (z_offset != 0) { Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform); } diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index d96eb1ae3ed94..4ccfa908e12d7 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -62,11 +62,11 @@ class LidarCenterPointNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 81f620018cf04..66e53310be263 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -48,7 +48,7 @@ bool NonMaximumSuppression::isTargetPairObject( } const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; - const auto sqr_dist_2d = autoware_universe_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 4799ec322d779..039d340cc2dc4 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -50,14 +50,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - autoware_universe_utils::pi / 2; + float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - autoware_universe_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - autoware_universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); if (has_variance) { obj.kinematics.has_position_covariance = has_variance; obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); @@ -102,7 +102,7 @@ uint8_t getSemanticType(const std::string & class_name) std::array convertPoseCovarianceMatrix(const Box3D & box3d) { - using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = box3d.x_variance; pose_covariance[POSE_IDX::Y_Y] = box3d.y_variance; @@ -113,7 +113,7 @@ std::array convertPoseCovarianceMatrix(const Box3D & box3d) std::array convertTwistCovarianceMatrix(const Box3D & box3d) { - using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array twist_covariance{}; twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index b65c70f02bd1b..f9687a6342c9c 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -117,8 +117,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint"); stop_watch_ptr_->tic("cyclic_time"); @@ -130,7 +130,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti rclcpp::shutdown(); } published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void LidarCenterPointNode::pointCloudCallback( diff --git a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp index cb387c404c07e..88a43139908a7 100644 --- a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp +++ b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp @@ -59,9 +59,9 @@ class LIDAR_CENTERPOINT_TVM_PUBLIC LidarCenterPointTVMNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace lidar_centerpoint_tvm diff --git a/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp b/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp index 16d7094e1e628..94639d71e66c0 100644 --- a/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp @@ -65,14 +65,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - autoware_universe_utils::pi / 2; + float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - autoware_universe_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - autoware_universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); // twist if (has_twist) { diff --git a/perception/lidar_centerpoint_tvm/src/node.cpp b/perception/lidar_centerpoint_tvm/src/node.cpp index fec19a479a89f..9cf82b1c6bcae 100644 --- a/perception/lidar_centerpoint_tvm/src/node.cpp +++ b/perception/lidar_centerpoint_tvm/src/node.cpp @@ -94,8 +94,8 @@ LidarCenterPointTVMNode::LidarCenterPointTVMNode(const rclcpp::NodeOptions & nod // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint_tvm"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp index d57405b1c3b7b..a82013fac6ce7 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp @@ -63,10 +63,10 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_pub_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr published_time_pub_{nullptr}; }; } // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp index d454932935b64..0a9ea413dc30d 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp @@ -83,7 +83,7 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT std::unique_ptr network_trt_ptr_{nullptr}; std::unique_ptr vg_ptr_{nullptr}; - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; std::unique_ptr pre_ptr_{nullptr}; std::unique_ptr post_ptr_{nullptr}; diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index a8b4ddbfd5db9..dd12c52970d38 100644 --- a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -49,7 +49,7 @@ bool NonMaximumSuppression::isTargetPairObject( } const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; - const auto sqr_dist_2d = autoware_universe_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/lidar_transfusion/lib/ros_utils.cpp index cdfa693af2219..ef5c45c339b64 100644 --- a/perception/lidar_transfusion/lib/ros_utils.cpp +++ b/perception/lidar_transfusion/lib/ros_utils.cpp @@ -49,14 +49,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = box3d.yaw + autoware_universe_utils::pi / 2; + float yaw = box3d.yaw + autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - autoware_universe_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - autoware_universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); } uint8_t getSemanticType(const std::string & class_name) diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/lidar_transfusion/lib/transfusion_trt.cpp index 4a8416a16e18c..d940b83c12cb4 100644 --- a/perception/lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/lidar_transfusion/lib/transfusion_trt.cpp @@ -38,7 +38,7 @@ TransfusionTRT::TransfusionTRT( network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); vg_ptr_ = std::make_unique(densification_param, config_, stream_); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); stop_watch_ptr_->tic("processing/inner"); initPtr(); diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp index 07b19f1db51f8..e3ea6b3780de8 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -94,12 +94,12 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) objects_pub_ = this->create_publisher( "~/output/objects", rclcpp::QoS(1)); - published_time_pub_ = std::make_unique(this); + published_time_pub_ = std::make_unique(this); // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic"); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 08d36c80d4cf8..3019f16ada7c9 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -56,7 +56,7 @@ class TrackerDebugger rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr processing_time_publisher_; rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; diagnostic_updater::Updater diagnostic_updater_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index b24eadd586e73..4c1ccced1bcf7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -73,7 +73,7 @@ class MultiObjectTracker : public rclcpp::Node // debugger std::unique_ptr debugger_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index d033e98cce46e..013137aa8b82c 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -52,8 +52,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { const double measurement_yaw = - autoware_universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = autoware_universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + autoware::universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware::universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -170,7 +170,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = autoware_universe_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -183,7 +183,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = autoware_universe_utils::getArea(measurement_object.shape); + const double area = autoware::universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index bb5fdef9be690..a3ece17eab112 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -24,7 +24,7 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ // initialize debug publishers if (debug_settings_.publish_processing_time) { processing_time_publisher_ = - std::make_unique(&node_, "multi_object_tracker"); + std::make_unique(&node_, "multi_object_tracker"); } if (debug_settings_.publish_tentative_objects) { diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index af61cfb8259b1..54255e706e35a 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -200,7 +200,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) debugger_ = std::make_unique(*this, world_frame_id_); debugger_->setObjectChannels(input_names_short); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void MultiObjectTracker::onTrigger() diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index fa7adb8396612..05e06107c2247 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -57,9 +57,9 @@ BicycleTracker::BicycleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - double r_stddev_x = 0.5; // in vehicle coordinate [m] - double r_stddev_y = 0.4; // in vehicle coordinate [m] - double r_stddev_yaw = autoware_universe_utils::deg2rad(30); // in map coordinate [rad] + double r_stddev_x = 0.5; // in vehicle coordinate [m] + double r_stddev_y = 0.4; // in vehicle coordinate [m] + double r_stddev_yaw = autoware::universe_utils::deg2rad(30); // in map coordinate [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -102,8 +102,8 @@ BicycleTracker::BicycleTracker( // Set motion limits { - constexpr double max_vel = autoware_universe_utils::kmph2mps(80); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -126,7 +126,7 @@ BicycleTracker::BicycleTracker( constexpr double p0_stddev_x = 0.8; // in object coordinate [m] constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = - autoware_universe_utils::deg2rad(25); // in map coordinate [rad] + autoware::universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -145,7 +145,7 @@ BicycleTracker::BicycleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - autoware_universe_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -153,7 +153,7 @@ BicycleTracker::BicycleTracker( const double slip = 0.0; const double p0_stddev_slip = - autoware_universe_utils::deg2rad(5); // in object coordinate [rad/s] + autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -328,7 +328,7 @@ bool BicycleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 92815f5e33a6b..4414d8a21ca01 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -58,10 +58,10 @@ BigVehicleTracker::BigVehicleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = autoware_universe_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = autoware::universe_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -70,7 +70,7 @@ BigVehicleTracker::BigVehicleTracker( // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = autoware_universe_utils::kmph2mps(10); // [m/s] + velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -120,8 +120,8 @@ BigVehicleTracker::BigVehicleTracker( // Set motion limits { - constexpr double max_vel = autoware_universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -144,7 +144,7 @@ BigVehicleTracker::BigVehicleTracker( constexpr double p0_stddev_x = 1.5; // in object coordinate [m] constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = - autoware_universe_utils::deg2rad(25); // in map coordinate [rad] + autoware::universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -163,7 +163,7 @@ BigVehicleTracker::BigVehicleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - autoware_universe_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -171,7 +171,7 @@ BigVehicleTracker::BigVehicleTracker( const double slip = 0.0; const double p0_stddev_slip = - autoware_universe_utils::deg2rad(5); // in object coordinate [rad/s] + autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -423,7 +423,7 @@ bool BigVehicleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 892b993a75348..1fff1a13add65 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -58,10 +58,10 @@ NormalVehicleTracker::NormalVehicleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = autoware_universe_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = autoware::universe_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -70,7 +70,7 @@ NormalVehicleTracker::NormalVehicleTracker( // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = autoware_universe_utils::kmph2mps(10); // [m/s] + velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -121,8 +121,8 @@ NormalVehicleTracker::NormalVehicleTracker( // Set motion limits { - constexpr double max_vel = autoware_universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -145,7 +145,7 @@ NormalVehicleTracker::NormalVehicleTracker( constexpr double p0_stddev_x = 1.0; // in object coordinate [m] constexpr double p0_stddev_y = 0.3; // in object coordinate [m] constexpr double p0_stddev_yaw = - autoware_universe_utils::deg2rad(25); // in map coordinate [rad] + autoware::universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -164,7 +164,7 @@ NormalVehicleTracker::NormalVehicleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - autoware_universe_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -172,7 +172,7 @@ NormalVehicleTracker::NormalVehicleTracker( const double slip = 0.0; const double p0_stddev_slip = - autoware_universe_utils::deg2rad(5); // in object coordinate [rad/s] + autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -425,7 +425,7 @@ bool NormalVehicleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 005d3c15df4df..887810066f038 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -57,9 +57,9 @@ PedestrianTracker::PedestrianTracker( // Initialize parameters // measurement noise covariance - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = autoware_universe_utils::deg2rad(30); // [rad] + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = autoware::universe_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -86,18 +86,18 @@ PedestrianTracker::PedestrianTracker( // Set motion model parameters { - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = autoware_universe_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = autoware_universe_utils::deg2rad(30); // [rad/(s*s)] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } // Set motion limits motion_model_.setMotionLimits( - autoware_universe_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30.0 /* [deg/s] maximum turn rate */ + autoware::universe_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ ); // Set initial state @@ -121,7 +121,7 @@ PedestrianTracker::PedestrianTracker( constexpr double p0_stddev_x = 2.0; // in object coordinate [m] constexpr double p0_stddev_y = 2.0; // in object coordinate [m] constexpr double p0_stddev_yaw = - autoware_universe_utils::deg2rad(1000); // in map coordinate [rad] + autoware::universe_utils::deg2rad(1000); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -140,9 +140,9 @@ PedestrianTracker::PedestrianTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - autoware_universe_utils::kmph2mps(120); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(120); // in object coordinate [m/s] constexpr double p0_stddev_wz = - autoware_universe_utils::deg2rad(360); // in object coordinate [rad/s] + autoware::universe_utils::deg2rad(360); // in object coordinate [rad/s] vel_cov = std::pow(p0_stddev_vel, 2.0); wz_cov = std::pow(p0_stddev_wz, 2.0); } else { @@ -323,7 +323,7 @@ bool PedestrianTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 52952c7820396..8e484623ef0aa 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -67,8 +67,8 @@ UnknownTracker::UnknownTracker( // Set motion limits motion_model_.setMotionLimits( - autoware_universe_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ - autoware_universe_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + autoware::universe_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + autoware::universe_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ ); // Set initial state @@ -107,8 +107,8 @@ UnknownTracker::UnknownTracker( } if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vx = autoware_universe_utils::kmph2mps(10); // [m/s] - constexpr double p0_stddev_vy = autoware_universe_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vx = autoware::universe_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = autoware::universe_utils::kmph2mps(10); // [m/s] const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index bf040d417e6bb..f21855c2356ef 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -60,8 +60,8 @@ void BicycleMotionModel::setDefaultParams() lr_min); // set motion limitations - constexpr double max_vel = autoware_universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30.0; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30.0; // [deg] maximum slip angle setMotionLimits(max_vel, max_slip); // set prediction parameters @@ -79,13 +79,13 @@ void BicycleMotionModel::setMotionParams( // set process noise covariance parameters motion_params_.q_stddev_acc_long = q_stddev_acc_long; motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; - motion_params_.q_stddev_yaw_rate_min = autoware_universe_utils::deg2rad(q_stddev_yaw_rate_min); - motion_params_.q_stddev_yaw_rate_max = autoware_universe_utils::deg2rad(q_stddev_yaw_rate_max); + motion_params_.q_stddev_yaw_rate_min = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_min); + motion_params_.q_stddev_yaw_rate_max = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_max); motion_params_.q_cov_slip_rate_min = - std::pow(autoware_universe_utils::deg2rad(q_stddev_slip_rate_min), 2.0); + std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_min), 2.0); motion_params_.q_cov_slip_rate_max = - std::pow(autoware_universe_utils::deg2rad(q_stddev_slip_rate_max), 2.0); - motion_params_.q_max_slip_angle = autoware_universe_utils::deg2rad(q_max_slip_angle); + std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_max), 2.0); + motion_params_.q_max_slip_angle = autoware::universe_utils::deg2rad(q_max_slip_angle); constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { @@ -103,7 +103,7 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_slip = autoware_universe_utils::deg2rad(max_slip); + motion_params_.max_slip = autoware::universe_utils::deg2rad(max_slip); } bool BicycleMotionModel::initialize( @@ -252,7 +252,7 @@ bool BicycleMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware_universe_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index af05590e3b05b..79ce6e5dbb61c 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -40,17 +40,17 @@ CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger(" void CTRVMotionModel::setDefaultParams() { // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = autoware_universe_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = autoware_universe_utils::deg2rad(30); // [rad/(s*s)] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); // set motion limitations - constexpr double max_vel = autoware_universe_utils::kmph2mps(10); // [m/s] maximum velocity - constexpr double max_wz = 30.0; // [deg] maximum yaw rate + constexpr double max_vel = autoware::universe_utils::kmph2mps(10); // [m/s] maximum velocity + constexpr double max_wz = 30.0; // [deg] maximum yaw rate setMotionLimits(max_vel, max_wz); // set prediction parameters @@ -74,7 +74,7 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_wz = autoware_universe_utils::deg2rad(max_wz); + motion_params_.max_wz = autoware::universe_utils::deg2rad(max_wz); } bool CTRVMotionModel::initialize( @@ -220,7 +220,7 @@ bool CTRVMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware_universe_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index 5ce944b0a79bb..8b3e9014f52d7 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -49,8 +49,8 @@ void CVMotionModel::setDefaultParams() setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); // set motion limitations - constexpr double max_vx = autoware_universe_utils::kmph2mps(60); // [m/s] maximum x velocity - constexpr double max_vy = autoware_universe_utils::kmph2mps(60); // [m/s] maximum y velocity + constexpr double max_vx = autoware::universe_utils::kmph2mps(60); // [m/s] maximum x velocity + constexpr double max_vy = autoware::universe_utils::kmph2mps(60); // [m/s] maximum y velocity setMotionLimits(max_vx, max_vy); // set prediction parameters diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 26fccca21aa22..2f5918a9c7f17 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -84,10 +84,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node } overlapped_judge_param_; // debug publisher - std::unique_ptr processing_time_publisher_; - std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_association diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp index 634be73508a2d..c32fe480ff4af 100644 --- a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -31,8 +31,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -133,7 +133,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = autoware_universe_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index cc1069321f6f8..5077268731955 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -43,7 +43,7 @@ bool isUnknownObjectOverlapped( const double distance_threshold = distance_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); - const double sq_distance = autoware_universe_utils::calcSquaredDistance2d( + const double sq_distance = autoware::universe_utils::calcSquaredDistance2d( unknown_object.kinematics.pose_with_covariance.pose, known_object.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; @@ -121,13 +121,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio // Debug publisher processing_time_publisher_ = - std::make_unique(this, "object_association_merger"); + std::make_unique(this, "object_association_merger"); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp index 4d635cf330fa6..e15871d18840f 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp @@ -74,7 +74,7 @@ void ObjectVelocitySplitterNode::onObjects(const DetectedObjects::ConstSharedPtr for (const auto & object : objects_data_->objects) { if ( - std::abs(autoware_universe_utils::calcNorm( + std::abs(autoware::universe_utils::calcNorm( object.kinematics.twist_with_covariance.twist.linear)) < node_param_.velocity_threshold) { low_speed_objects.objects.emplace_back(object); } else { diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index bed7f84ab79a2..ae6d7a34fca2a 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -120,9 +120,9 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node // Debugger std::shared_ptr debugger_ptr_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; // ROS Parameters std::string map_frame_; diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 37811f4f6fb08..f62a3f7e689fc 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -73,7 +73,7 @@ geometry_msgs::msg::PoseStamped getPoseStamped( RCLCPP_WARN_THROTTLE( rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what()); } - return autoware_universe_utils::transform2pose(tf_stamped); + return autoware::universe_utils::transform2pose(tf_stamped); } boost::optional getCost( @@ -225,8 +225,8 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "occupancy_grid_map_outlier_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp index d09b1d9782604..84ca13c8b1881 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -85,8 +85,8 @@ class GridMapFusionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr fused_map_pub_; rclcpp::Publisher::SharedPtr single_frame_pub_; std::vector::SharedPtr> grid_map_subs_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; // Topics manager std::size_t num_input_topics_{1}; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 7c3b7b3e4b1a3..1d119102dc28d 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -68,8 +68,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node rclcpp::Publisher::SharedPtr occupancy_grid_map_pub_; message_filters::Subscriber obstacle_pointcloud_sub_; message_filters::Subscriber raw_pointcloud_sub_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; std::shared_ptr tf2_{std::make_shared(get_clock())}; std::shared_ptr tf2_listener_{std::make_shared(*tf2_)}; diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 211479ed24e6b..fa93db802b0ce 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -142,8 +142,8 @@ GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) // debug tools { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index f66ca3c7fc472..d4e177209f99d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -55,9 +55,9 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = autoware_universe_utils::deg2rad(-180.0); - constexpr double max_angle = autoware_universe_utils::deg2rad(180.0); - constexpr double angle_increment = autoware_universe_utils::deg2rad(0.1); + constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0); + constexpr double max_angle = autoware::universe_utils::deg2rad(180.0); + constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 0a16c488158f1..79fc6e8c40b5e 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -56,9 +56,9 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = autoware_universe_utils::deg2rad(-180.0); - constexpr double max_angle = autoware_universe_utils::deg2rad(180.0); - constexpr double angle_increment = autoware_universe_utils::deg2rad(0.1); + constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0); + constexpr double max_angle = autoware::universe_utils::deg2rad(180.0); + constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 2f3a97c82ccc8..d019280aefda0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -119,8 +119,8 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "pointcloud_based_occupancy_grid_map"); diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 8e9a8431a4258..209207fe10f34 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -50,7 +50,7 @@ void transformPointcloud( const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose, sensor_msgs::msg::PointCloud2 & output) { - const auto transform = autoware_universe_utils::pose2transform(pose); + const auto transform = autoware::universe_utils::pose2transform(pose); Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); pcl_ros::transformPointCloud(tf_matrix, input, output); @@ -96,7 +96,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform( target_frame, source_header.frame_id, source_header.stamp, rclcpp::Duration::from_seconds(0.5)); - pose = autoware_universe_utils::transform2pose(tf_stamped.transform); + pose = autoware::universe_utils::transform2pose(tf_stamped.transform); return pose; } @@ -108,7 +108,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); - pose = autoware_universe_utils::transform2pose(tf_stamped.transform); + pose = autoware::universe_utils::transform2pose(tf_stamped.transform); return pose; } diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp index 339cb8afa454b..2f5884c04ce37 100644 --- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp @@ -125,13 +125,13 @@ rcl_interfaces::msg::SetParametersResult RadarCrossingObjectsNoiseFilterNode::on bool RadarCrossingObjectsNoiseFilterNode::isNoise(const DetectedObject & object) { const double velocity = std::abs( - autoware_universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear)); + autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear)); const double object_angle = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double object_position_angle = std::atan2( object.kinematics.pose_with_covariance.pose.position.y, object.kinematics.pose_with_covariance.pose.position.x); const double crossing_yaw = - autoware_universe_utils::normalizeRadian(object_angle - object_position_angle); + autoware::universe_utils::normalizeRadian(object_angle - object_position_angle); if ( velocity > node_param_.velocity_threshold && diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp index 501edcbe84af8..44ccaae8ee7fc 100644 --- a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -48,9 +48,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) { rclcpp::init(0, nullptr); { - auto velocity = autoware_universe_utils::createVector3(40.0, 30.0, 0.0); - auto position = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(40.0, 30.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -81,9 +81,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = autoware_universe_utils::createVector3(40.0, 30.0, 0.0); - auto position = autoware_universe_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(40.0, 30.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -112,9 +112,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = autoware_universe_utils::createVector3(24.0, 18.0, 0.0); - auto position = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(24.0, 18.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -143,9 +143,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = autoware_universe_utils::createVector3(24.0, 18.0, 0.0); - auto position = autoware_universe_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(24.0, 18.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp index ef6891c6c8944..cd7e7de6b2ff4 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp @@ -32,10 +32,10 @@ namespace radar_fusion_to_detected_object { +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::Point2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 85940b7f99f3d..5a0c1b5412312 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -29,10 +29,10 @@ namespace radar_fusion_to_detected_object { +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::Point2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; @@ -136,7 +136,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( bool RadarFusionToDetectedObject::hasTwistCovariance( const TwistWithCovariance & twist_with_covariance) { - using IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto covariance = twist_with_covariance.covariance; if (covariance[IDX::X_X] == 0.0 && covariance[IDX::Y_Y] == 0.0 && covariance[IDX::Z_Z] == 0.0) { return false; @@ -151,11 +151,11 @@ bool RadarFusionToDetectedObject::isYawCorrect( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance, const double & yaw_threshold) { - const double twist_yaw = autoware_universe_utils::normalizeRadian( + const double twist_yaw = autoware::universe_utils::normalizeRadian( std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x)); - const double object_yaw = autoware_universe_utils::normalizeRadian( + const double object_yaw = autoware::universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - const double diff_yaw = autoware_universe_utils::normalizeRadian(twist_yaw - object_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(twist_yaw - object_yaw); if (std::abs(diff_yaw) < yaw_threshold) { return true; } else if (M_PI - yaw_threshold < std::abs(diff_yaw)) { @@ -174,12 +174,12 @@ RadarFusionToDetectedObject::filterRadarWithinObject( { std::vector outputs{}; - autoware_universe_utils::Point2d object_size{ + autoware::universe_utils::Point2d object_size{ object.shape.dimensions.x, object.shape.dimensions.y}; LinearRing2d object_box = createObject2dWithMargin(object_size, param_.bounding_box_margin); - object_box = autoware_universe_utils::transformVector( + object_box = autoware::universe_utils::transformVector( object_box, - autoware_universe_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); + autoware::universe_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); for (const auto & radar : (*radars)) { Point2d radar_point{ @@ -214,10 +214,10 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( Eigen::Vector2d vec_min_distance(0.0, 0.0); if (param_.velocity_weight_min_distance > 0.0) { auto comp_func = [&](const RadarInput & a, const RadarInput & b) { - return autoware_universe_utils::calcSquaredDistance2d( + return autoware::universe_utils::calcSquaredDistance2d( a.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position) < - autoware_universe_utils::calcSquaredDistance2d( + autoware::universe_utils::calcSquaredDistance2d( b.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position); }; diff --git a/perception/radar_object_clustering/README.md b/perception/radar_object_clustering/README.md index abd2b6f9d1bfe..8f936ce61a1f3 100644 --- a/perception/radar_object_clustering/README.md +++ b/perception/radar_object_clustering/README.md @@ -83,13 +83,13 @@ These are used in `isSameObject` function as below. bool RadarObjectClusteringNode::isSameObject( const DetectedObject & object_1, const DetectedObject & object_2) { - const double angle_diff = std::abs(autoware_universe_utils::normalizeRadian( + const double angle_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); const double velocity_diff = std::abs( object_1.kinematics.twist_with_covariance.twist.linear.x - object_2.kinematics.twist_with_covariance.twist.linear.x); - const double distance = autoware_universe_utils::calcDistance2d( + const double distance = autoware::universe_utils::calcDistance2d( object_1.kinematics.pose_with_covariance.pose.position, object_2.kinematics.pose_with_covariance.pose.position); diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp index 1681260c711b2..339ab21741d29 100644 --- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -179,13 +179,13 @@ void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr bool RadarObjectClusteringNode::isSameObject( const DetectedObject & object_1, const DetectedObject & object_2) { - const double angle_diff = std::abs(autoware_universe_utils::normalizeRadian( + const double angle_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); const double velocity_diff = std::abs( object_1.kinematics.twist_with_covariance.twist.linear.x - object_2.kinematics.twist_with_covariance.twist.linear.x); - const double distance = autoware_universe_utils::calcDistance2d( + const double distance = autoware::universe_utils::calcDistance2d( object_1.kinematics.pose_with_covariance.pose.position, object_2.kinematics.pose_with_covariance.pose.position); diff --git a/perception/radar_object_tracker/src/data_association/data_association.cpp b/perception/radar_object_tracker/src/data_association/data_association.cpp index ddfb0c3e8f76d..b308f62a24dfa 100644 --- a/perception/radar_object_tracker/src/data_association/data_association.cpp +++ b/perception/radar_object_tracker/src/data_association/data_association.cpp @@ -55,8 +55,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { const double measurement_yaw = - autoware_universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = autoware_universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + autoware::universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware::universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -203,7 +203,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = autoware_universe_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -221,7 +221,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = autoware_universe_utils::getArea(measurement_object.shape); + const double area = autoware::universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) { passed_gate = false; } diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index 0548002606d5d..e1603fbfab702 100644 --- a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -209,7 +209,7 @@ void ConstantTurnRateMotionTracker::loadDefaultModelParameters(const std::string // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = autoware_universe_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vx_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [m/s] } bool ConstantTurnRateMotionTracker::predict(const rclcpp::Time & time) @@ -393,7 +393,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose( Eigen::MatrixXd Y_yaw = Eigen::MatrixXd::Zero(1, 1); const auto yaw = [&] { - auto obj_yaw = autoware_universe_utils::normalizeRadian( + auto obj_yaw = autoware::universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); while (M_PI_2 <= yaw_state - obj_yaw) { obj_yaw = obj_yaw + M_PI; @@ -617,7 +617,7 @@ bool ConstantTurnRateMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index 754ff88861b0d..1bc548fa7a951 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -237,8 +237,8 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = autoware_universe_utils::kmph2mps(max_speed_kmph); // [m/s] - max_vy_ = autoware_universe_utils::kmph2mps(max_speed_kmph); // [rad/s] + max_vx_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vy_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [rad/s] } bool LinearMotionTracker::predict(const rclcpp::Time & time) @@ -682,7 +682,7 @@ bool LinearMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp index fa1acda54e4fe..ba2500a60beff 100644 --- a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp +++ b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -81,10 +81,10 @@ bool checkCloseLaneletCondition( double object_motion_yaw = object_yaw; bool velocity_is_reverted = object.kinematics.twist_with_covariance.twist.linear.x < 0.0; if (velocity_is_reverted) { - object_motion_yaw = autoware_universe_utils::normalizeRadian(object_yaw + M_PI); + object_motion_yaw = autoware::universe_utils::normalizeRadian(object_yaw + M_PI); } const double delta_yaw = object_motion_yaw - lane_yaw; - const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw > max_angle_diff_from_lane) { @@ -134,14 +134,14 @@ bool hasValidVelocityDirectionToLanelet( const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); const double object_vel_yaw_global = - autoware_universe_utils::normalizeRadian(object_yaw + object_vel_yaw); + autoware::universe_utils::normalizeRadian(object_yaw + object_vel_yaw); const double object_vel = std::hypot(object_vel_x, object_vel_y); for (const auto & lanelet : lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_vel_yaw_global - lane_yaw; - const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double lane_vel = object_vel * std::sin(normalized_delta_yaw); if (std::fabs(lane_vel) < max_lateral_velocity) { diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 8db732245b1fd..30d02c90b0594 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -64,7 +64,7 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_radar_{}; rclcpp::Subscription::SharedPtr sub_odometry_{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onRadarTracks(const RadarTracks::ConstSharedPtr msg); diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 0caf29df2735e..36f4d3cd3da90 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -92,7 +92,7 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1)); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // Publisher pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1); @@ -261,7 +261,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() } // yaw angle (vehicle heading) is obtained from ground velocity - double yaw = autoware_universe_utils::normalizeRadian( + double yaw = autoware::universe_utils::normalizeRadian( std::atan2(compensated_velocity.y, compensated_velocity.x)); // kinematics setting @@ -273,7 +273,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() // velocity of object is defined in the object coordinate // heading is obtained from ground velocity kinematics.pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); // longitudinal velocity is the length of the velocity vector // lateral velocity is zero, use default value kinematics.twist_with_covariance.twist.linear.x = std::sqrt( @@ -334,8 +334,8 @@ geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocityEgoM std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = autoware_universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; @@ -351,8 +351,8 @@ std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = autoware_universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array twist_covariance{}; twist_covariance[POSE_IDX::X_X] = radar_track.velocity_covariance[RADAR_IDX::X_X]; twist_covariance[POSE_IDX::X_Y] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; @@ -368,8 +368,8 @@ std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatri std::array RadarTracksMsgsConverterNode::convertAccelerationCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = autoware_universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array acceleration_covariance{}; acceleration_covariance[POSE_IDX::X_X] = radar_track.acceleration_covariance[RADAR_IDX::X_X]; acceleration_covariance[POSE_IDX::X_Y] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; diff --git a/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp b/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp index 5e010b21478fc..b3ebc4d916947 100644 --- a/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp +++ b/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp @@ -68,9 +68,9 @@ class LowIntensityClusterFilter : public rclcpp::Node utils::FilterTargetLabel filter_target_; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace low_intensity_cluster_filter diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp index cae09d3cb20e7..4772c355ec0b6 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp @@ -52,8 +52,8 @@ LowIntensityClusterFilter::LowIntensityClusterFilter(const rclcpp::NodeOptions & "output/objects", rclcpp::QoS{1}); // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "low_intensity_cluster_filter_node"); @@ -85,8 +85,8 @@ void LowIntensityClusterFilter::objectCallback( geometry_msgs::msg::Pose max_pose; max_pose.position.x = max_x_; max_pose.position.y = max_y_; - auto min_ranged_transformed = autoware_universe_utils::transformPose(min_range, transform_stamp); - auto max_range_transformed = autoware_universe_utils::transformPose(max_pose, transform_stamp); + auto min_ranged_transformed = autoware::universe_utils::transformPose(min_range, transform_stamp); + auto max_range_transformed = autoware::universe_utils::transformPose(max_pose, transform_stamp); for (const auto & feature_object : input_msg->feature_objects) { const auto & object = feature_object.object; const auto & label = object.classification.front().label; diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index 953736e0924ce..1332647b0f1f4 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -246,9 +246,9 @@ bool correctWithDefaultValue( // correct to set long length is x, short length is y if (shape.dimensions.x < shape.dimensions.y) { - geometry_msgs::msg::Vector3 rpy = autoware_universe_utils::getRPY(pose.orientation); + geometry_msgs::msg::Vector3 rpy = autoware::universe_utils::getRPY(pose.orientation); rpy.z = rpy.z + M_PI_2; - pose.orientation = autoware_universe_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); + pose.orientation = autoware::universe_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); double temp = shape.dimensions.x; shape.dimensions.x = shape.dimensions.y; shape.dimensions.y = temp; diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 119b0d22b3f7d..5719cd97b3ed5 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -54,13 +54,13 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); processing_time_publisher_ = - std::make_unique(this, "shape_estimation"); + std::make_unique(this, "shape_estimation"); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } static autoware_perception_msgs::msg::ObjectClassification::_label_type get_label( @@ -114,7 +114,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared if (use_vehicle_reference_yaw_ && is_vehicle) { ref_yaw_info = ReferenceYawInfo{ static_cast(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)), - autoware_universe_utils::deg2rad(10)}; + autoware::universe_utils::deg2rad(10)}; } if (use_vehicle_reference_shape_size_ && is_vehicle) { ref_shape_size_info = ReferenceShapeSizeInfo{object.shape, ReferenceShapeSizeInfo::Mode::Min}; diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index db2a8b9ddaedd..807f12e39b025 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -35,11 +35,11 @@ class ShapeEstimationNode : public rclcpp::Node // ros rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp index 73ccd33464d81..d25eef20cc676 100644 --- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp +++ b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp @@ -47,7 +47,7 @@ class SimpleObjectMergerNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_objects_{}; std::vector::SharedPtr> sub_objects_array{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number); diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index af8f02caef72b..faa531560ba09 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -98,7 +98,7 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ } // Subscriber - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_objects_array.resize(input_topic_size); objects_data_.resize(input_topic_size); diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp index 885852106a82c..6044148a932ae 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -63,8 +63,8 @@ class TrtYoloXNode : public rclcpp::Node LabelMap label_map_; std::unique_ptr trt_yolox_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index 4eb266445489e..4ee18b99e4bc6 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -31,9 +31,9 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) { { stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); debug_publisher_ = - std::make_unique(this, "tensorrt_yolox"); + std::make_unique(this, "tensorrt_yolox"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index be03ec3868af9..3683ba02b38cd 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -86,8 +86,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // debug object publisher rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> @@ -106,7 +106,7 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // tracker default settings TrackerStateParameter tracker_state_parameter_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // merge policy (currently not used) struct diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp index ae4c05013b7db..4d10a415df7da 100644 --- a/perception/tracking_object_merger/src/data_association/data_association.cpp +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -31,8 +31,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -183,7 +183,7 @@ double DataAssociation::calcScoreBetweenObjects( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = autoware_universe_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index d1bc61d90edac..99dccab3ff72c 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -155,14 +155,14 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("radar-radar", data_association_map_); // debug publisher - processing_time_publisher_ = std::make_unique( + processing_time_publisher_ = std::make_unique( this, "decorative_object_merger_node"); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 7d8b5a4cc02f3..e981ef09a21b0 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -280,7 +280,7 @@ bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const Track // diff of motion yaw angle const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw); const auto normalized_motion_yaw_diff = - autoware_universe_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi + autoware::universe_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi // evaluate if motion yaw angle is same constexpr double yaw_threshold = M_PI / 4.0; // 45 deg if (std::abs(normalized_motion_yaw_diff) < yaw_threshold) { @@ -305,7 +305,7 @@ bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); // calc yaw diff const auto yaw_diff = std::fabs(main_yaw - sub_yaw); - const auto normalized_yaw_diff = autoware_universe_utils::normalizeRadian(yaw_diff); // -pi ~ pi + const auto normalized_yaw_diff = autoware::universe_utils::normalizeRadian(yaw_diff); // -pi ~ pi // evaluate if yaw is reverted constexpr double yaw_threshold = M_PI / 2.0; // 90 deg if (std::abs(normalized_yaw_diff) >= yaw_threshold) { diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index 7224db018ccff..7750bf642516a 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -512,9 +512,10 @@ void MapBasedDetector::getVisibleTrafficLights( double max_angle_range; if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { max_angle_range = - autoware_universe_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); + autoware::universe_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); } else { - max_angle_range = autoware_universe_utils::deg2rad(config_.car_traffic_light_max_angle_range); + max_angle_range = + autoware::universe_utils::deg2rad(config_.car_traffic_light_max_angle_range); } // traffic light bottom left const auto & tl_bl = traffic_light.front(); @@ -530,7 +531,7 @@ void MapBasedDetector::getVisibleTrafficLights( } // check angle range - const double tl_yaw = autoware_universe_utils::normalizeRadian( + const double tl_yaw = autoware::universe_utils::normalizeRadian( std::atan2(tl_br.y() - tl_bl.y(), tl_br.x() - tl_bl.x()) + M_PI_2); // get direction of z axis @@ -538,7 +539,7 @@ void MapBasedDetector::getVisibleTrafficLights( tf2::Matrix3x3 camera_rotation_matrix(tf_map2camera.getRotation()); camera_z_dir = camera_rotation_matrix * camera_z_dir; double camera_yaw = std::atan2(camera_z_dir.y(), camera_z_dir.x()); - camera_yaw = autoware_universe_utils::normalizeRadian(camera_yaw); + camera_yaw = autoware::universe_utils::normalizeRadian(camera_yaw); if (!isInAngleRange(tl_yaw, camera_yaw, max_angle_range)) { continue; } diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 8a1bea3303c67..6515a515397da 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -102,7 +102,7 @@ void CloudOcclusionPredictor::predict( pcl::PointCloud cloud_camera; // points within roi pcl::PointCloud cloud_roi; - autoware_universe_utils::transformPointCloudFromROSMsg(*cloud_msg, cloud_camera, camera2cloud); + autoware::universe_utils::transformPointCloudFromROSMsg(*cloud_msg, cloud_camera, camera2cloud); filterCloud(cloud_camera, roi_tls, roi_brs, cloud_roi); diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 11f05ad126bc1..70167f0833586 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -163,7 +163,7 @@ class FreespacePlannerNode : public rclcpp::Node TransformStamped getTransform(const std::string & from, const std::string & to); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::freespace_planner 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 b0cf556c49b5d..55068263f5d5b 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 @@ -142,8 +142,8 @@ Trajectory getPartialTrajectory( double calcDistance2d(const Trajectory & trajectory, const Pose & pose) { - const auto idx = autoware_motion_utils::findNearestIndex(trajectory.points, pose.position); - return autoware_universe_utils::calcDistance2d(trajectory.points.at(idx), pose); + const auto idx = autoware::motion_utils::findNearestIndex(trajectory.points, pose.position); + return autoware::universe_utils::calcDistance2d(trajectory.points.at(idx), pose); } Pose transformPose(const Pose & pose, const TransformStamped & transform) @@ -287,7 +287,7 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() @@ -365,7 +365,7 @@ bool FreespacePlannerNode::isPlanRequired() if (node_param_.replan_when_obstacle_found) { algo_->setMap(*occupancy_grid_); - const size_t nearest_index_partial = autoware_motion_utils::findNearestIndex( + 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; @@ -395,7 +395,7 @@ bool FreespacePlannerNode::isPlanRequired() void FreespacePlannerNode::updateTargetIndex() { const auto is_near_target = - autoware_universe_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < + autoware::universe_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < node_param_.th_arrived_distance_m; const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index dbab68be5f8be..2d47c7acb9801 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -21,8 +21,8 @@ namespace autoware::freespace_planning_algorithms { -using autoware_universe_utils::createQuaternionFromYaw; -using autoware_universe_utils::normalizeRadian; +using autoware::universe_utils::createQuaternionFromYaw; +using autoware::universe_utils::normalizeRadian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) @@ -97,7 +97,7 @@ double PlannerWaypoints::compute_length() const for (size_t i = 0; i < waypoints.size() - 1; ++i) { const auto pose_a = waypoints.at(i); const auto pose_b = waypoints.at(i + 1); - total_cost += autoware_universe_utils::calcDistance2d(pose_a.pose, pose_b.pose); + total_cost += autoware::universe_utils::calcDistance2d(pose_a.pose, pose_b.pose); } return total_cost; } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 5aa7f9889ccf8..6d093b3c7c0cd 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -42,7 +42,7 @@ double calcReedsSheppDistance( void setYaw(geometry_msgs::msg::Quaternion * orientation, const double yaw) { - *orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + *orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); } geometry_msgs::msg::Pose calcRelativePose( @@ -209,7 +209,7 @@ double AstarSearch::estimateCost(const geometry_msgs::msg::Pose & pose) const total_cost += calcReedsSheppDistance(pose, goal_pose_, radius) * astar_param_.distance_heuristic_weight; } else { - total_cost += autoware_universe_utils::calcDistance2d(pose, goal_pose_) * + total_cost += autoware::universe_utils::calcDistance2d(pose, goal_pose_) * astar_param_.distance_heuristic_weight; } return total_cost; @@ -332,7 +332,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const const double lateral_goal_range = planner_common_param_.lateral_goal_range / 2.0; const double longitudinal_goal_range = planner_common_param_.longitudinal_goal_range / 2.0; const double goal_angle = - autoware_universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); + autoware::universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); const auto relative_pose = calcRelativePose(goal_pose_, node2pose(node)); @@ -348,7 +348,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const } const auto angle_diff = - autoware_universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); if (std::abs(angle_diff) > goal_angle) { return false; } @@ -363,7 +363,7 @@ geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const pose_local.position.x = node.x; pose_local.position.y = node.y; pose_local.position.z = goal_pose_.position.z; - pose_local.orientation = autoware_universe_utils::createQuaternionFromYaw(node.theta); + pose_local.orientation = autoware::universe_utils::createQuaternionFromYaw(node.theta); return pose_local; } 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 edce59e4f473b..69c52b9c2eafb 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -108,7 +108,7 @@ geometry_msgs::msg::Pose get_closest_centerline_pose( const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); - const auto nearest_idx = autoware_motion_utils::findNearestIndex( + const auto nearest_idx = autoware::motion_utils::findNearestIndex( convertCenterlineToPoints(closest_lanelet), point.position); const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; @@ -190,13 +190,13 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } const std_msgs::msg::ColorRGBA cl_route = - autoware_universe_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15); + autoware::universe_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15); const std_msgs::msg::ColorRGBA cl_ll_borders = - autoware_universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); + autoware::universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); const std_msgs::msg::ColorRGBA cl_end = - autoware_universe_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05); + autoware::universe_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05); const std_msgs::msg::ColorRGBA cl_goal = - autoware_universe_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05); + autoware::universe_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05); visualization_msgs::msg::MarkerArray route_marker_array; insert_marker_array( @@ -216,27 +216,27 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( - autoware_universe_utils::LinearRing2d goal_footprint) const + autoware::universe_utils::LinearRing2d goal_footprint) const { visualization_msgs::msg::MarkerArray msg; - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP, - autoware_universe_utils::createMarkerScale(0.05, 0.0, 0.0), - autoware_universe_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); + autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); marker.lifetime = rclcpp::Duration::from_seconds(2.5); marker.points.push_back( - autoware_universe_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); marker.points.push_back( - autoware_universe_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); marker.points.push_back( - autoware_universe_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); marker.points.push_back( - autoware_universe_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); marker.points.push_back( - autoware_universe_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); marker.points.push_back( - autoware_universe_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -247,7 +247,7 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( bool DefaultPlanner::check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelet & combined_prev_lanelet, - const autoware_universe_utils::Polygon2d & goal_footprint, double & next_lane_length, + const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, const double search_margin) { // check if goal footprint is in current lane @@ -302,8 +302,8 @@ bool DefaultPlanner::is_goal_valid( shoulder_lanelets, goal, &closest_shoulder_lanelet)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = autoware_universe_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = autoware_universe_utils::deg2rad(param_.goal_angle_threshold_deg); + const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -332,8 +332,8 @@ bool DefaultPlanner::is_goal_valid( } const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - autoware_universe_utils::LinearRing2d goal_footprint = - transformVector(local_vehicle_footprint, autoware_universe_utils::pose2transform(goal)); + autoware::universe_utils::LinearRing2d goal_footprint = + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(goal)); pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); @@ -357,9 +357,9 @@ bool DefaultPlanner::is_goal_valid( if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = autoware_universe_utils::normalizeRadian(lane_yaw - goal_yaw); + const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = autoware_universe_utils::deg2rad(param_.goal_angle_threshold_deg); + const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index 0b7d889a24f83..60c2a53b82123 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -52,7 +52,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; MarkerArray visualize_debug_footprint( - autoware_universe_utils::LinearRing2d goal_footprint_) const; + autoware::universe_utils::LinearRing2d goal_footprint_) const; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: @@ -86,7 +86,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin bool check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelet & combined_prev_lanelet, - const autoware_universe_utils::Polygon2d & goal_footprint, double & next_lane_length, + const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, const double search_margin = 2.0); /** diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 904e618f825fb..5046b81c72b96 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -21,10 +21,10 @@ #include #include -autoware_universe_utils::Polygon2d convert_linear_ring_to_polygon( - autoware_universe_utils::LinearRing2d footprint) +autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( + autoware::universe_utils::LinearRing2d footprint) { - autoware_universe_utils::Polygon2d footprint_polygon; + autoware::universe_utils::Polygon2d footprint_polygon; boost::geometry::append(footprint_polygon.outer(), footprint[0]); boost::geometry::append(footprint_polygon.outer(), footprint[1]); boost::geometry::append(footprint_polygon.outer(), footprint[2]); @@ -120,7 +120,7 @@ geometry_msgs::msg::Pose convertBasicPoint3dToPose( pose.position.y = point.y(); pose.position.z = point.z(); - pose.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); + pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); return pose; } diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index 04f235a58206f..c8812e2c76dd6 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -42,8 +42,8 @@ bool exists(const std::vector & vectors, const T & item) return false; } -autoware_universe_utils::Polygon2d convert_linear_ring_to_polygon( - autoware_universe_utils::LinearRing2d footprint); +autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( + autoware::universe_utils::LinearRing2d footprint); void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); diff --git a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp index b0af480d10357..da14712dd6c64 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp @@ -26,7 +26,7 @@ namespace autoware::mission_planner ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node) { const double angle_deg = node->declare_parameter("arrival_check_angle_deg"); - angle_ = autoware_universe_utils::deg2rad(angle_deg); + angle_ = autoware::universe_utils::deg2rad(angle_deg); distance_ = node->declare_parameter("arrival_check_distance"); duration_ = node->declare_parameter("arrival_check_duration"); } @@ -67,14 +67,14 @@ bool ArrivalChecker::is_arrived(const PoseStamped & pose) const } // Check distance. - if (distance_ < autoware_universe_utils::calcDistance2d(pose.pose, goal.pose)) { + if (distance_ < autoware::universe_utils::calcDistance2d(pose.pose, goal.pose)) { return false; } // Check angle. const double yaw_pose = tf2::getYaw(pose.pose.orientation); const double yaw_goal = tf2::getYaw(goal.pose.orientation); - const double yaw_diff = autoware_universe_utils::normalizeRadian(yaw_pose - yaw_goal); + const double yaw_diff = autoware::universe_utils::normalizeRadian(yaw_pose - yaw_goal); if (angle_ < std::fabs(yaw_diff)) { return false; } 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 0b892d7998078..708b3de40bdab 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp @@ -42,7 +42,7 @@ class ArrivalChecker double duration_; std::optional goal_with_uuid_; rclcpp::Subscription::SharedPtr sub_goal_; - autoware_motion_utils::VehicleStopChecker vehicle_stop_checker_; + autoware::motion_utils::VehicleStopChecker vehicle_stop_checker_; }; } // namespace autoware::mission_planner diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index fc90b91f6d996..e484b9915aa87 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -78,7 +78,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto period = rclcpp::Rate(10).period(); data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void MissionPlanner::publish_pose_log(const Pose & pose, const std::string & pose_type) diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index ab108d07918d2..1a04a91c14ba3 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -85,7 +85,7 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_reroute_availability_{this, "~/input/reroute_availability"}; rclcpp::Subscription::SharedPtr sub_vector_map_; @@ -132,7 +132,7 @@ class MissionPlanner : public rclcpp::Node double minimum_reroute_length_; bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::mission_planner diff --git a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp index 8dba7731649b4..038006517f45a 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp @@ -22,7 +22,7 @@ std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float a const float g = static_cast((code << 48) >> 56) / 255.0; const float b = static_cast((code << 56) >> 56) / 255.0; - return autoware_universe_utils::createMarkerColor(r, g, b, alpha); + return autoware::universe_utils::createMarkerColor(r, g, b, alpha); } } // namespace diff --git a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp index 4cbf6924abf36..42d92bd0c91da 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp @@ -20,8 +20,8 @@ using geometry_msgs::msg::Point; using std_msgs::msg::ColorRGBA; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerScale; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -66,9 +66,9 @@ Marker createCircleMarker( for (size_t i = 0; i < num_points; ++i) { Point point; const double ratio = static_cast(i) / static_cast(num_points); - const double theta = 2 * autoware_universe_utils::pi * ratio; - point.x = data.pose.position.x + radius * autoware_universe_utils::cos(theta); - point.y = data.pose.position.y + radius * autoware_universe_utils::sin(theta); + const double theta = 2 * autoware::universe_utils::pi * ratio; + point.x = data.pose.position.x + radius * autoware::universe_utils::cos(theta); + point.y = data.pose.position.y + radius * autoware::universe_utils::sin(theta); point.z = data.pose.position.z + height + height_offset; marker.points.push_back(point); } 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 a7f4787f76c48..e4ae670ef6b97 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 @@ -63,7 +63,7 @@ struct Obstacle twist(object.kinematics.initial_twist_with_covariance.twist), twist_reliable(true), classification(object.classification.at(0)), - uuid(autoware_universe_utils::toHexString(object.object_id)), + uuid(autoware::universe_utils::toHexString(object.object_id)), shape(object.shape), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) @@ -74,7 +74,7 @@ struct Obstacle } } - Polygon2d toPolygon() const { return autoware_universe_utils::toPolygon2d(pose, shape); } + Polygon2d toPolygon() const { return autoware::universe_utils::toPolygon2d(pose, shape); } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. geometry_msgs::msg::Pose pose; // interpolated with the current stamp @@ -198,33 +198,33 @@ struct LongitudinalInfo void onParam(const std::vector & parameters) { - autoware_universe_utils::updateParam(parameters, "normal.max_accel", max_accel); - autoware_universe_utils::updateParam(parameters, "normal.min_accel", min_accel); - autoware_universe_utils::updateParam(parameters, "normal.max_jerk", max_jerk); - autoware_universe_utils::updateParam(parameters, "normal.min_jerk", min_jerk); - autoware_universe_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); - autoware_universe_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); - autoware_universe_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); - autoware_universe_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam(parameters, "normal.max_accel", max_accel); + autoware::universe_utils::updateParam(parameters, "normal.min_accel", min_accel); + autoware::universe_utils::updateParam(parameters, "normal.max_jerk", max_jerk); + autoware::universe_utils::updateParam(parameters, "normal.min_jerk", min_jerk); + autoware::universe_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); + autoware::universe_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); + autoware::universe_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); + autoware::universe_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + autoware::universe_utils::updateParam( parameters, "common.slow_down_min_accel", slow_down_min_accel); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.slow_down_min_jerk", slow_down_min_jerk); - autoware_universe_utils::updateParam(parameters, "common.idling_time", idling_time); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam(parameters, "common.idling_time", idling_time); + autoware::universe_utils::updateParam( parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.safe_distance_margin", safe_distance_margin); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold); } @@ -265,7 +265,7 @@ struct DebugData MarkerArray stop_wall_marker; MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; - std::vector detection_polygons; + std::vector detection_polygons; }; struct EgoNearestParam @@ -280,21 +280,22 @@ struct EgoNearestParam TrajectoryPoint calcInterpolatedPoint( const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const { - return autoware_motion_utils::calcInterpolatedPoint( - autoware_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 autoware_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 autoware_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/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 5d0d99a742d1c..dd9a6c3880672 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -145,11 +145,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr traj_sub_; - autoware_universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber objects_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; - autoware_universe_utils::InterProcessPollingSubscriber acc_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; // Vehicle Parameters @@ -161,7 +161,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node PlanningAlgorithm planning_algorithm_; // stop watch - mutable autoware_universe_utils::StopWatch< + mutable autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; mutable std::shared_ptr debug_data_ptr_{nullptr}; @@ -274,9 +274,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector prev_closest_stop_obstacles_{}; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_planning 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 525a2cfca6ece..0641d076bb3ff 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 @@ -125,7 +125,7 @@ class PlannerInterface bool suppress_sudden_obstacle_stop_; // stop watch - autoware_universe_utils::StopWatch< + autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; @@ -172,7 +172,7 @@ class PlannerInterface const geometry_msgs::msg::Pose & ego_pose) const { const auto & p = ego_nearest_param_; - return autoware_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 autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj_points, ego_pose, p.dist_threshold, p.yaw_threshold); } @@ -303,29 +303,29 @@ class PlannerInterface if (obstacle_to_param_struct_map.count(label + "." + movement_postfix) < 1) continue; auto & param_by_obstacle_label = obstacle_to_param_struct_map.at(label + "." + movement_postfix); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin", param_by_obstacle_label.max_lat_margin); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".min_lat_margin", param_by_obstacle_label.min_lat_margin); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".max_ego_velocity", param_by_obstacle_label.max_ego_velocity); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".min_ego_velocity", param_by_obstacle_label.min_ego_velocity); } } // common parameters - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.lpf_gain_slow_down_vel", lpf_gain_slow_down_vel); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.lpf_gain_lat_dist", lpf_gain_lat_dist); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } @@ -385,15 +385,15 @@ class PlannerInterface if (type_str == "default") { continue; } - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".sudden_object_acc_threshold", param.sudden_object_acc_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".sudden_object_dist_threshold", param.sudden_object_dist_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); param.sudden_object_acc_threshold = diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp index e9c661d575733..fa6ee17d117be 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp @@ -30,8 +30,8 @@ namespace polygon_utils { namespace bg = boost::geometry; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; Polygon2d createOneStepPolygon( const std::vector & last_poses, diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index bc9f6070e159d..5721efd051f63 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -60,7 +60,7 @@ using tier4_planning_msgs::msg::VelocityLimitClearCommand; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index ceb5da13882bd..8af9a63f3a1fa 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -62,7 +62,7 @@ size_t getIndexWithLongitudinalOffset( if (longitudinal_offset > 0) { for (size_t i = *start_idx; i < points.size() - 1; ++i) { const double segment_length = - autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); sum_length += segment_length; if (sum_length >= longitudinal_offset) { const double back_length = sum_length - longitudinal_offset; @@ -79,7 +79,7 @@ size_t getIndexWithLongitudinalOffset( for (size_t i = *start_idx; 0 < i; --i) { const double segment_length = - autoware_universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); + autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); sum_length += segment_length; if (sum_length >= -longitudinal_offset) { const double back_length = sum_length + longitudinal_offset; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 8acdfea013e12..9682ac63104c8 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -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 = autoware_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 = - autoware_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; } @@ -83,12 +83,12 @@ double calcDiffAngleAgainstTrajectory( const std::vector & traj_points, const geometry_msgs::msg::Pose & target_pose) { const size_t nearest_idx = - autoware_motion_utils::findNearestIndex(traj_points, target_pose.position); + 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); - const double diff_yaw = autoware_universe_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); return diff_yaw; } @@ -96,7 +96,7 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { const size_t object_idx = - autoware_motion_utils::findNearestIndex(traj_points, obstacle.pose.position); + 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); @@ -132,7 +132,7 @@ TrajectoryPoint getExtendTrajectoryPoint( const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = autoware_universe_utils::calcOffsetPose( + extend_trajectory_point.pose = autoware::universe_utils::calcOffsetPose( goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; @@ -146,7 +146,7 @@ std::vector extendTrajectoryPoints( { auto output_points = input_points; const auto is_driving_forward_opt = - autoware_motion_utils::isDrivingForwardWithTwist(input_points); + 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()) { @@ -189,12 +189,12 @@ std::vector getTargetObjectType(rclcpp::Node & node, const std::string & pa std::vector resampleTrajectoryPoints( const std::vector & traj_points, const double interval) { - 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); + 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) +geometry_msgs::msg::Point toGeomPoint(const autoware::universe_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; geom_point.x = point.x(); @@ -281,75 +281,75 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( const std::vector & parameters) { // behavior determination - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", crossing_obstacle_velocity_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold", crossing_obstacle_traj_angle_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop.crossing_obstacle.collision_time_margin", collision_time_margin); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold", outside_obstacle_min_velocity_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold", ego_obstacle_overlap_time_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check", max_prediction_time_for_collision_check); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop_obstacle_hold_time_threshold", stop_obstacle_hold_time_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.prediction_resampling_time_interval", prediction_resampling_time_interval); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.prediction_resampling_time_horizon", prediction_resampling_time_horizon); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin_against_unknown", max_lat_margin_for_stop_against_unknown); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.lat_distance_threshold", yield_lat_distance_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles", max_lat_dist_between_obstacles); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold", stopped_obstacle_velocity_threshold); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time", max_obstacles_collision_time); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.lat_hysteresis_margin", lat_hysteresis_margin_for_slow_down); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition", successive_num_to_entry_slow_down_condition); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", successive_num_to_exit_slow_down_condition); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", enable_to_consider_current_pose); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.time_to_convergence", time_to_convergence); } @@ -440,9 +440,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -461,17 +461,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( { planner_ptr_->onParam(parameters); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.enable_debug_info", enable_debug_info_); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.stop_on_curve.additional_safe_distance_margin", additional_safe_distance_margin_on_curve_); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.stop_on_curve.min_safe_distance_margin", min_safe_distance_margin_on_curve_); @@ -480,7 +480,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); behavior_determination_param_.onParam(parameters); @@ -504,7 +504,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto & objects = *objects_ptr; const auto & acc = *acc_ptr; - const auto traj_points = autoware_motion_utils::convertToTrajectoryPointArray(*msg); + const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready if (traj_points.empty()) { return; @@ -513,7 +513,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms stop_watch_.tic(__func__); *debug_data_ptr_ = DebugData(); - const auto is_driving_forward = autoware_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 @@ -546,7 +546,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 7. Publish trajectory const auto output_traj = - autoware_motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); + autoware::motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); trajectory_pub_->publish(output_traj); // 8. Publish debug data @@ -573,10 +573,10 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const double vehicle_width = vehicle_info.vehicle_width_m; const size_t nearest_idx = - autoware_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); + autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); const double current_ego_lat_error = current_ego_pose_error.position.y; const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); double time_elapsed{0.0}; @@ -592,11 +592,11 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence; geometry_msgs::msg::Pose indexed_pose_err; indexed_pose_err.set__orientation( - autoware_universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); indexed_pose_err.set__position( - autoware_universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); current_poses.push_back( - autoware_universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { time_elapsed += p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); @@ -610,23 +610,23 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { boost::geometry::append( idx_poly, - autoware_universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) + autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) .outer()); boost::geometry::append( - idx_poly, autoware_universe_utils::fromMsg( - autoware_universe_utils::calcOffsetPose( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) .position) .to_2d()); boost::geometry::append( - idx_poly, autoware_universe_utils::fromMsg( - autoware_universe_utils::calcOffsetPose( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) .position) .to_2d()); } else { boost::geometry::append( - idx_poly, autoware_universe_utils::toFootprint( + idx_poly, autoware::universe_utils::toFootprint( pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) .outer()); } @@ -655,7 +655,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::vector target_obstacles; for (const auto & predicted_object : objects.objects) { const auto & object_id = - autoware_universe_utils::toHexString(predicted_object.object_id).substr(0, 4); + autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); const auto & current_obstacle_pose = obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); @@ -683,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 = - autoware_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); @@ -749,10 +749,10 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( const size_t first_collision_idx) const { const auto obstacle_idx = - autoware_motion_utils::findNearestIndex(traj_points, obstacle.pose.position); + autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose.position); const double obstacle_to_col_points_distance = - autoware_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. @@ -944,7 +944,7 @@ std::optional ObstacleCruisePlannerNode::createYieldCruiseObstac } const auto collision_points = [&]() -> std::optional> { - const auto obstacle_idx = autoware_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; @@ -1358,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 = autoware_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; @@ -1373,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 = autoware_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; @@ -1396,7 +1396,7 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&prev_closest_stop_obstacle](const PredictedObject & po) { - return autoware_universe_utils::toHexString(po.object_id) == + return autoware::universe_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid; }); // If previous closest obstacle disappear from the perception result, do nothing anymore. @@ -1456,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(autoware_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)); @@ -1531,10 +1531,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } } for (size_t i = 0; i < stop_collision_points.size(); ++i) { - auto collision_point_marker = autoware_universe_utils::createDefaultMarker( + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "cruise_collision_points", i, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = stop_collision_points.at(i); debug_marker.markers.push_back(collision_point_marker); } @@ -1547,10 +1547,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision point - auto collision_point_marker = autoware_universe_utils::createDefaultMarker( + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "stop_collision_points", 0, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; debug_marker.markers.push_back(collision_point_marker); } @@ -1564,16 +1564,16 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision points - auto front_collision_point_marker = autoware_universe_utils::createDefaultMarker( + auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); front_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; - auto back_collision_point_marker = autoware_universe_utils::createDefaultMarker( + auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); back_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; @@ -1590,10 +1590,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } { // footprint polygons - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", now(), "detection_polygons", 0, Marker::LINE_LIST, - autoware_universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { @@ -1602,16 +1602,16 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); marker.points.push_back( - autoware_universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); marker.points.push_back( - autoware_universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); } } debug_marker.markers.push_back(marker); } // slow down debug wall marker - autoware_universe_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); debug_marker_pub_->publish(debug_marker); 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 b409ed9239f7e..d256a64984667 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 @@ -174,7 +174,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Publish Debug trajectories const double traj_front_to_vehicle_offset = - autoware_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 = - autoware_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); } @@ -227,7 +227,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( std::vector resampled_opt_position; for (size_t i = closest_idx; i < stop_traj_points.size(); ++i) { const double query_s = std::max( - autoware_motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); + 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 - autoware_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 = autoware_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 = autoware_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 = autoware_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 = autoware_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,15 +442,15 @@ std::optional OptimizationBasedPlanner::getSBoundaries( if (min_slow_down_idx) { const auto & current_time = planner_data.current_time; - const auto marker_pose = autoware_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 = autoware_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); + autoware::universe_utils::appendMarkerArray(markers, &wall_msg); // publish rviz marker debug_wall_marker_pub_->publish(wall_msg); @@ -583,7 +583,7 @@ bool OptimizationBasedPlanner::checkOnTrajectory( } const double lateral_offset = - std::fabs(autoware_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 = autoware_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 = autoware_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()); @@ -627,7 +627,7 @@ geometry_msgs::msg::Pose OptimizationBasedPlanner::transformBaseLink2Center( geometry_msgs::msg::Pose center_pose; center_pose.position = - autoware_universe_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); + autoware::universe_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); center_pose.orientation = pose_base_link.orientation; return center_pose; 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 56201229cdee6..345265b1d20b0 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 @@ -313,13 +313,13 @@ std::vector PIDBasedPlanner::planCruise( const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( stop_traj_points, dist_to_rss_wall, ego_idx); - auto markers = autoware_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. markers.markers.front().color = - autoware_universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); - autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); @@ -345,8 +345,8 @@ std::vector PIDBasedPlanner::planCruise( // delete marker const auto markers = - autoware_motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); - autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + 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 autoware_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 = autoware_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 = autoware_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; @@ -599,7 +599,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( double sum_dist = 0.0; for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { if (i != ego_seg_idx) { - sum_dist += autoware_universe_utils::calcDistance2d( + sum_dist += autoware::universe_utils::calcDistance2d( acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); } @@ -622,7 +622,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( void PIDBasedPlanner::updateCruiseParam(const std::vector & parameters) { - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); { // velocity limit based planner @@ -632,26 +632,26 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki = p.pid_vel_controller->getKi(); double kd = p.pid_vel_controller->getKd(); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kp", kp); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.ki", ki); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kd", kd); p.pid_vel_controller->updateParam(kp, ki, kd); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", p.output_ratio_during_accel); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", p.disable_target_acceleration); @@ -665,11 +665,11 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_acc = p.pid_acc_controller->getKi(); double kd_acc = p.pid_acc_controller->getKd(); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); @@ -678,32 +678,32 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_jerk = p.pid_jerk_controller->getKi(); double kd_jerk = p.pid_jerk_controller->getKd(); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", p.output_acc_ratio_during_accel); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", p.output_jerk_ratio_during_accel); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); } // min_cruise_target_vel - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 5ab0ba237ec78..ab6d4650eb3d5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -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 = autoware_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,14 +218,14 @@ double calcDecelerationVelocityFromDistanceToTarget( std::vector resampleTrajectoryPoints( const std::vector & traj_points, const double interval) { - 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); + 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) +autoware::universe_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) { - return autoware_universe_utils::Point2d{p.x, p.y}; + return autoware::universe_utils::Point2d{p.x, p.y}; } } // namespace @@ -248,8 +248,8 @@ std::vector PlannerInterface::generateStopTrajectory( // delete marker const auto markers = - autoware_motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + 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; return planner_data.traj_points; @@ -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 = - autoware_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 = autoware_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,11 @@ 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 = autoware_motion_utils::searchZeroVelocityIndex( + 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 = - autoware_motion_utils::calcSignedArcLength( + 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); @@ -329,7 +329,7 @@ std::vector PlannerInterface::generateStopTrajectory( } const double acceptable_stop_pos = - autoware_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()); @@ -356,8 +356,8 @@ std::vector PlannerInterface::generateStopTrajectory( if (!determined_zero_vel_dist) { // delete marker const auto markers = - autoware_motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + 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; return planner_data.traj_points; @@ -370,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 = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - const double diff_dist_front_points = autoware_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); @@ -387,13 +387,13 @@ std::vector PlannerInterface::generateStopTrajectory( // Insert stop point auto output_traj_points = planner_data.traj_points; const auto zero_vel_idx = - autoware_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 = autoware_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); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // Publish Stop Reason @@ -406,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() > - autoware_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; @@ -448,7 +448,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( : std::abs(vehicle_info_.min_longitudinal_offset_m); // calculate short trajectory points towards obstacle - const size_t obj_segment_idx = autoware_motion_utils::findNearestSegmentIndex( + 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}; @@ -460,7 +460,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { break; } - sum_short_traj_length += autoware_universe_utils::calcDistance2d( + sum_short_traj_length += autoware::universe_utils::calcDistance2d( planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); } std::reverse(short_traj_points.begin(), short_traj_points.end()); @@ -471,15 +471,15 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate collision index between straight line from ego pose and object const auto calculate_distance_from_straight_ego_path = [&](const auto & ego_pose, const auto & object_polygon) { - const auto forward_ego_pose = autoware_universe_utils::calcOffsetPose( + const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose( ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); - const auto ego_straight_segment = autoware_universe_utils::Segment2d{ + const auto ego_straight_segment = autoware::universe_utils::Segment2d{ convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; return boost::geometry::distance(ego_straight_segment, object_polygon); }; const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); const auto object_polygon = - autoware_universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); const auto collision_idx = [&]() -> std::optional { for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { const double dist_to_obj = calculate_distance_from_straight_ego_path( @@ -499,7 +499,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate margin from obstacle const double partial_segment_length = [&]() { - const double collision_segment_length = autoware_universe_utils::calcDistance2d( + const double collision_segment_length = autoware::universe_utils::calcDistance2d( resampled_short_traj_points.at(*collision_idx - 1), resampled_short_traj_points.at(*collision_idx)); const double prev_dist = calculate_distance_from_straight_ego_path( @@ -512,7 +512,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( const double short_margin_from_obstacle = partial_segment_length + - autoware_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_; @@ -532,9 +532,9 @@ double PlannerInterface::calcDistanceToCollisionPoint( ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const size_t collision_segment_idx = - autoware_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 = autoware_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); @@ -553,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 autoware_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 @@ -563,7 +563,7 @@ 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 = - autoware_motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); + 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 @@ -648,25 +648,25 @@ std::vector PlannerInterface::generateSlowDownTrajectory( return *slow_down_end_idx; }(); - const auto markers = autoware_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); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } // add debug virtual wall if (slow_down_start_idx) { - const auto markers = autoware_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( + autoware::universe_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { - const auto markers = autoware_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( + autoware::universe_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } @@ -728,9 +728,9 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( // calculate distance to collision points const double dist_to_front_collision = - autoware_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 = - autoware_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; @@ -773,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 = - autoware_motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); + autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); const double prev_dist_to_slow_down = - autoware_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 1a032c9f5603f..8c3abe58f5d59 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -35,7 +35,7 @@ PointWithStamp calcNearestCollisionPoint( std::vector dist_vec; for (const auto & collision_point : collision_points) { - const double dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( segment_points, 0, collision_point.point); dist_vec.push_back(dist); } @@ -51,10 +51,10 @@ std::optional>> getCollisionIndex( const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(object_pose, object_shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = - autoware_universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); if (approximated_dist > max_dist) { continue; } @@ -105,14 +105,14 @@ std::optional> getCollisionPoint( const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m : vehicle_info.min_longitudinal_offset_m; - const auto bumper_pose = autoware_universe_utils::calcOffsetPose( + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( traj_points.at(collision_info->first).pose, x_diff_to_bumper, 0.0, 0.0); std::optional max_collision_length = std::nullopt; std::optional max_collision_point = std::nullopt; for (const auto & poly_vertex : collision_info->second) { const double dist_from_bumper = - std::abs(autoware_universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { max_collision_length = dist_from_bumper; @@ -121,7 +121,7 @@ std::optional> getCollisionPoint( } return std::make_pair( *max_collision_point, - autoware_motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - *max_collision_length); } diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index b95fb6e5d29f9..8162d8036d9dc 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -59,10 +59,10 @@ visualization_msgs::msg::Marker getObjectMarker( { const auto current_time = rclcpp::Clock().now(); - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, - autoware_universe_utils::createMarkerScale(2.0, 2.0, 2.0), - autoware_universe_utils::createMarkerColor(r, g, b, 0.8)); + autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0), + autoware::universe_utils::createMarkerColor(r, g, b, 0.8)); marker.pose = obj_pose; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index df9dec3cf3b75..7f2688ffaad95 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -89,7 +89,7 @@ struct TimeKeeper double accumulated_time{0.0}; - autoware_universe_utils::StopWatch< + autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -122,7 +122,7 @@ struct TrajectoryParam void onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; // common updateParam( @@ -145,7 +145,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index cece0142bc29a..4303e5c7e05b4 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -93,9 +93,9 @@ struct ReferencePoint geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const { - auto pose_with_deviation = autoware_universe_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); + auto pose_with_deviation = autoware::universe_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - autoware_universe_utils::createQuaternionFromYaw(getYaw() + yaw_dev); + autoware::universe_utils::createQuaternionFromYaw(getYaw() + yaw_dev); return pose_with_deviation; } }; 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 d7356c0e43fc4..359c20f2a4d29 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -52,7 +52,7 @@ class PathOptimizer : public rclcpp::Node public: bool isDrivingForward(const std::vector & path_points) { - const auto is_driving_forward = autoware_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_; } @@ -90,7 +90,7 @@ class PathOptimizer : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - autoware_universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ this, "~/input/odometry"}; // debug publisher @@ -137,9 +137,9 @@ class PathOptimizer : public rclcpp::Node private: double vehicle_stop_margin_outside_drivable_area_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::path_optimizer 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 e53cf37890691..edc91bd40d4dc 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 @@ -36,14 +36,14 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils namespace autoware::path_optimizer { @@ -52,8 +52,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = autoware_universe_utils::getPoint(t1); - const auto p2 = autoware_universe_utils::getPoint(t2); + const auto p1 = autoware::universe_utils::getPoint(t1); + const auto p2 = autoware::universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { 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 acc40573ef542..a54d60835aadf 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 @@ -35,7 +35,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); @@ -45,7 +45,7 @@ geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint template <> double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p); -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils namespace autoware::path_optimizer { @@ -61,14 +61,14 @@ std::optional getPointIndexAfter( } double sum_length = - -autoware_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}; // search forward if (sum_length < min_offset) { 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)); + sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (min_offset < sum_length) { output_idx = i - 1; } @@ -82,7 +82,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < min_offset) { output_idx = i - 1; } @@ -98,7 +98,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware_universe_utils::getPose(point); + traj_point.pose = autoware::universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -109,9 +109,9 @@ template <> inline TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point) { TrajectoryPoint traj_point; - traj_point.pose = autoware_universe_utils::getPose(ref_point); + traj_point.pose = autoware::universe_utils::getPose(ref_point); traj_point.longitudinal_velocity_mps = - autoware_universe_utils::getLongitudinalVelocity(ref_point); + autoware::universe_utils::getLongitudinalVelocity(ref_point); return traj_point; } @@ -144,7 +144,7 @@ size_t findEgoIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return autoware_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 autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -173,13 +173,13 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, autoware_universe_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - autoware_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"), @@ -187,7 +187,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = autoware_universe_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index eea6ada555044..397c334eca5b7 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -20,10 +20,10 @@ namespace autoware::path_optimizer { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; namespace { @@ -53,16 +53,16 @@ MarkerArray getFootprintsMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware_universe_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position); marker.points.push_back(marker.points.front()); @@ -110,7 +110,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; - const auto lb = autoware_universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware::universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); lb_marker.points.push_back(lb); @@ -132,7 +132,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; - const auto ub = autoware_universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware::universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); ub_marker.points.push_back(ub); @@ -167,11 +167,11 @@ MarkerArray getBoundsLineMarkerArray( const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; - const auto ub = autoware_universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware::universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; - const auto lb = autoware_universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware::universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } marker_array.markers.push_back(ub_marker); @@ -205,22 +205,22 @@ MarkerArray getVehicleCircleLinesMarkerArray( // apply lateral and yaw deviation auto pose_with_deviation = - autoware_universe_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); + autoware::universe_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - autoware_universe_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); + autoware::universe_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); for (const double d : vehicle_circle_longitudinal_offsets) { // apply longitudinal offset - auto base_pose = autoware_universe_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); + auto base_pose = autoware::universe_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); + autoware::universe_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const auto ub = - autoware_universe_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - autoware_universe_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -247,7 +247,7 @@ MarkerArray getCurrentVehicleCirclesMarkerArray( "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = autoware_universe_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); + marker.pose = autoware::universe_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -289,7 +289,7 @@ MarkerArray getVehicleCirclesMarkerArray( "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = autoware_universe_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); + marker.pose = autoware::universe_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -356,15 +356,15 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware_universe_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); + autoware::universe_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0) + autoware::universe_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0) + autoware::universe_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); + autoware::universe_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 7ecb7543d0463..a5da50862c9fb 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -125,8 +125,8 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { const double base_theta = tf2::getYaw(pose.orientation); - const double target_theta = autoware_universe_utils::calcAzimuthAngle(pose.position, target_pos); - const double diff_theta = autoware_universe_utils::normalizeRadian(target_theta - base_theta); + const double target_theta = autoware::universe_utils::calcAzimuthAngle(pose.position, target_pos); + const double diff_theta = autoware::universe_utils::normalizeRadian(target_theta - base_theta); return diff_theta > 0; } @@ -141,18 +141,18 @@ double calcLateralDistToBounds( const double max_lat_offset = is_left_bound ? max_lat_offset_for_left : -max_lat_offset_for_left; const double min_lat_offset = is_left_bound ? min_lat_offset_for_left : -min_lat_offset_for_left; const auto max_lat_offset_point = - autoware_universe_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; const auto min_lat_offset_point = - autoware_universe_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; double closest_dist_to_bound = max_lat_offset; for (size_t i = 0; i < bound.size() - 1; ++i) { - const auto intersect_point = autoware_universe_utils::intersect( + const auto intersect_point = autoware::universe_utils::intersect( min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); if (intersect_point) { const bool is_point_left = isLeft(pose, *intersect_point); const double dist_to_bound = - autoware_universe_utils::calcDistance2d(pose.position, *intersect_point) * + autoware::universe_utils::calcDistance2d(pose.position, *intersect_point) * (is_point_left ? 1.0 : -1.0); // the bound which is closest to the centerline will be chosen @@ -283,7 +283,7 @@ MPTOptimizer::MPTParam::MPTParam( void MPTOptimizer::MPTParam::onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; { // option updateParam(parameters, "mpt.option.steer_limit_constraint", steer_limit_constraint); @@ -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 = autoware_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 = autoware_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 = autoware_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); @@ -623,7 +623,7 @@ void MPTOptimizer::updateOrientation( const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); for (size_t i = 0; i < ref_points.size(); ++i) { ref_points.at(i).pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw_vec.at(i)); + autoware::universe_utils::createQuaternionFromYaw(yaw_vec.at(i)); } } @@ -691,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points ref_points.at(i).delta_arc_length = (i == ref_points.size() - 1) ? 0.0 - : autoware_universe_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); + : autoware::universe_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); } } @@ -704,14 +704,14 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c const auto front_wheel_pos = trajectory_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); - const bool are_too_close_points = autoware_universe_utils::calcDistance2d( + const bool are_too_close_points = autoware::universe_utils::calcDistance2d( front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; const auto front_wheel_yaw = are_too_close_points ? ref_points.at(i).getYaw() - : autoware_universe_utils::calcAzimuthAngle( + : autoware::universe_utils::calcAzimuthAngle( ref_points.at(i).pose.position, front_wheel_pos); ref_points.at(i).alpha = - autoware_universe_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); + autoware::universe_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); } { // avoidance @@ -771,10 +771,10 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c if (prev_ref_points_ptr_ && !prev_ref_points_ptr_->empty()) { for (int i = 0; i < static_cast(ref_points.size()); ++i) { const size_t prev_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, autoware_universe_utils::getPose(ref_points.at(i)), + *prev_ref_points_ptr_, autoware::universe_utils::getPose(ref_points.at(i)), ego_nearest_param_); - const double dist_to_prev = autoware_universe_utils::calcDistance2d( + const double dist_to_prev = autoware::universe_utils::calcDistance2d( ref_points.at(i), prev_ref_points_ptr_->at(prev_idx)); if (max_dist_threshold < dist_to_prev) { continue; @@ -1081,7 +1081,7 @@ void MPTOptimizer::avoidSuddenSteering( return; } const size_t prev_ego_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, autoware_universe_utils::getPose(ref_points.front()), + *prev_ref_points_ptr_, autoware::universe_utils::getPose(ref_points.front()), ego_nearest_param_); const double max_bound_fixing_length = ego_vel * mpt_param_.max_bound_fixing_time; @@ -1129,11 +1129,11 @@ void MPTOptimizer::updateVehicleBounds( collision_check_pose.position.y - ref_point.pose.position.y, collision_check_pose.position.x - ref_point.pose.position.x); const double offset_y = - -autoware_universe_utils::calcDistance2d(ref_point, collision_check_pose) * + -autoware::universe_utils::calcDistance2d(ref_point, collision_check_pose) * std::sin(tmp_yaw - collision_check_yaw); const auto vehicle_bounds_pose = - autoware_universe_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); + autoware::universe_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); // interpolate bounds const auto bounds = [&]() { @@ -1587,7 +1587,7 @@ Eigen::VectorXd MPTOptimizer::calcInitialSolutionForManualWarmStart( Eigen::VectorXd u0 = Eigen::VectorXd::Zero(D_un); - const size_t nearest_idx = autoware_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 = autoware_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 = autoware_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 = autoware_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 46fb7d0d5bb95..3899867a9dcce 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -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 = autoware_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; @@ -75,7 +75,7 @@ std::vector calcSegmentLengthVector(const std::vector & std::vector segment_length_vector; for (size_t i = 0; i < points.size() - 1; ++i) { const double segment_length = - autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); segment_length_vector.push_back(segment_length); } return segment_length_vector; @@ -150,15 +150,15 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) set_param_res_ = this->add_on_set_parameters_callback( std::bind(&PathOptimizer::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; // parameters for option updateParam( @@ -246,7 +246,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); const auto output_traj_msg = - autoware_motion_utils::convertToTrajectory(traj_points, path_ptr->header); + 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; @@ -279,7 +279,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - autoware_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); } @@ -400,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 = autoware_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); @@ -453,14 +453,14 @@ void PathOptimizer::applyInputVelocity( // insert stop point explicitly const auto stop_idx = - autoware_motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + 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: autoware_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 = autoware_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); @@ -471,9 +471,9 @@ void PathOptimizer::applyInputVelocity( } if (*stop_seg_idx == output_traj_points.size() - 2) { const double signed_projected_length_to_segment = - autoware_motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( output_traj_points, *stop_seg_idx, input_stop_pose.position); - const double segment_length = autoware_motion_utils::calcSignedArcLength( + 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. @@ -533,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 = - autoware_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 = - autoware_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; } @@ -561,11 +561,11 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos { time_keeper_ptr_->tic(__func__); - auto virtual_wall_marker = autoware_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 = - autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.5); + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.5); } virtual_wall_pub_->publish(virtual_wall_marker); @@ -664,7 +664,7 @@ void PathOptimizer::publishDebugData(const Header & header) const // publish trajectories const auto debug_extended_traj = - autoware_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 9228631b80afa..11e31bfd2d459 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -39,7 +39,7 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam( parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); @@ -78,7 +78,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // ego pose is lost or new ego pose is designated in simulation const double delta_dist = - autoware_universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + autoware::universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -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 = autoware_motion_utils::calcLateralOffset( + 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 = - autoware_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,7 +174,7 @@ 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 = autoware_motion_utils::calcLongitudinalOffsetPoint( + const auto prev_forward_point = autoware::motion_utils::calcLongitudinalOffsetPoint( prev_traj_points, prev_ego_seg_idx, lon_dist); if (!prev_forward_point) { continue; @@ -182,9 +182,9 @@ bool ReplanChecker::isPathForwardChanged( // calculate lateral offset of current trajectory points to prev forward point const auto forward_seg_idx = - autoware_motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); - const double forward_lat_offset = - autoware_motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + autoware::motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = 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; } @@ -205,7 +205,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - autoware_universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware::universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index a94f8b4bbabe7..bed9bbfa500c6 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -39,17 +39,17 @@ namespace autoware::path_optimizer { namespace bg = boost::geometry; -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::LineString2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; namespace { geometry_msgs::msg::Point getStartPoint( const std::vector & bound, const geometry_msgs::msg::Point & point) { - const size_t segment_idx = autoware_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}; @@ -62,7 +62,7 @@ geometry_msgs::msg::Point getStartPoint( } const auto first_point = - autoware_motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); + autoware::motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); if (first_point) { return *first_point; } @@ -86,7 +86,7 @@ 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 = - autoware_motion_utils::calcLateralOffset(front_bound, point); + autoware::motion_utils::calcLateralOffset(front_bound, point); if (lat_dist_to_front_bound < min_dist) { return true; } @@ -136,13 +136,13 @@ bool isOutsideDrivableAreaFromRectangleFootprint( // calculate footprint corner points const auto top_left_pos = - autoware_universe_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; const auto top_right_pos = - autoware_universe_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; const auto bottom_right_pos = - autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; const auto bottom_left_pos = - autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; if (use_footprint_polygon_for_outside_drivable_area_check) { // calculate footprint polygon diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index a1b429ae2357e..433d1d0995088 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -33,7 +33,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p) @@ -52,7 +52,7 @@ double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & { return p.longitudinal_velocity_mps; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils namespace autoware::path_optimizer { @@ -108,12 +108,12 @@ void compensateLastPose( const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; - const double dist = - autoware_universe_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double dist = autoware::universe_utils::calcDistance2d( + last_path_point.pose.position, last_traj_pose.position); const double norm_diff_yaw = [&]() { const double diff_yaw = tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return autoware_universe_utils::normalizeRadian(diff_yaw); + return autoware::universe_utils::normalizeRadian(diff_yaw); }(); if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { traj_points.push_back(convertToTrajectoryPoint(last_path_point)); @@ -140,10 +140,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = autoware_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 autoware_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 = autoware_motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = autoware_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 autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } std::vector resampleReferencePoints( @@ -175,7 +175,7 @@ std::vector resampleReferencePoints( base_keys.push_back(0.0); } else { const double delta_arc_length = - autoware_universe_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); + autoware::universe_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); base_keys.push_back(base_keys.back() + delta_arc_length); } @@ -187,7 +187,7 @@ std::vector resampleReferencePoints( if (i == 0) { query_keys.push_back(0.0); } else { - const double delta_arc_length = autoware_universe_utils::calcDistance2d( + const double delta_arc_length = autoware::universe_utils::calcDistance2d( resampled_ref_points.at(i), resampled_ref_points.at(i - 1)); const double key = query_keys.back() + delta_arc_length; if (base_keys.back() < key) { @@ -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 = autoware_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/common_structs.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp index e26d13b94ecb0..0b7e28ee1cbb5 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp @@ -83,7 +83,7 @@ struct TimeKeeper double accumulated_time{0.0}; - autoware_universe_utils::StopWatch< + autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -100,7 +100,7 @@ struct CommonParam void onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; // common updateParam( @@ -123,7 +123,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } 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 6a2c3ef45f66b..2de74a4f14f4f 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 @@ -49,7 +49,7 @@ class ElasticBandSmoother : public rclcpp::Node public: bool isDrivingForward(const std::vector & path_points) { - const auto is_driving_forward = autoware_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_; } @@ -82,7 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - autoware_universe_utils::InterProcessPollingSubscriber odom_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber odom_sub_{ this, "~/input/odometry"}; // debug publisher @@ -113,9 +113,9 @@ class ElasticBandSmoother : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::path_smoother diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp index df9dbdf7f7e49..603ae8a91f944 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp @@ -24,8 +24,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = autoware_universe_utils::getPoint(t1); - const auto p2 = autoware_universe_utils::getPoint(t2); + const auto p1 = autoware::universe_utils::getPoint(t1); + const auto p2 = autoware::universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; return (std::abs(p1.x - p2.x) <= epsilon && std::abs(p1.y - p2.y) <= epsilon); 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 715a3c35609a1..77a9dbae81a0c 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 @@ -49,14 +49,14 @@ std::optional getPointIndexAfter( } double sum_length = - -autoware_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}; // search forward if (sum_length < min_offset) { 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)); + sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (min_offset < sum_length) { output_idx = i - 1; } @@ -70,7 +70,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < min_offset) { output_idx = i - 1; } @@ -86,7 +86,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware_universe_utils::getPose(point); + traj_point.pose = autoware::universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -109,7 +109,7 @@ size_t findEgoSegmentIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -128,13 +128,13 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, autoware_universe_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - autoware_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"), @@ -142,7 +142,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = autoware_universe_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index b0c5974eca036..4222e0fe98438 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -125,7 +125,7 @@ EBPathSmoother::EBParam::EBParam(rclcpp::Node * node) void EBPathSmoother::EBParam::onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; { // option updateParam(parameters, "elastic_band.option.enable_warm_start", enable_warm_start); @@ -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 = autoware_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 = - autoware_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__, " "); @@ -389,8 +389,8 @@ void EBPathSmoother::updateConstraint( } // publish fixed trajectory - const auto eb_fixed_traj = - autoware_motion_utils::convertToTrajectory(debug_fixed_traj_points, createHeader(clock_.now())); + const auto eb_fixed_traj = 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__, " "); @@ -443,12 +443,12 @@ std::optional> EBPathSmoother::convertOptimizedPoin auto eb_traj_point = traj_points.at(i); eb_traj_point.pose = - autoware_universe_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); eb_traj_points.push_back(eb_traj_point); } // update orientation - autoware_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 8814fb5f9e5fd..b92798f92728c 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -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 = autoware_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; @@ -105,15 +105,15 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; // parameters for ego nearest search ego_nearest_param_.onParam(parameters); @@ -170,7 +170,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); const auto output_traj_msg = - autoware_motion_utils::convertToTrajectory(traj_points, path_ptr->header); + 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); @@ -224,7 +224,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - autoware_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); @@ -261,12 +261,12 @@ void ElasticBandSmoother::applyInputVelocity( time_keeper_ptr_->tic(__func__); // crop forward for faster calculation - const double output_traj_length = autoware_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 autoware_motion_utils::cropForwardPoints( + return autoware::motion_utils::cropForwardPoints( input_traj_points, ego_pose.position, ego_seg_idx, output_traj_length + margin_traj_length); }(); @@ -289,14 +289,14 @@ void ElasticBandSmoother::applyInputVelocity( // insert stop point explicitly const auto stop_idx = - autoware_motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + 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: autoware_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 = autoware_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); @@ -307,9 +307,9 @@ void ElasticBandSmoother::applyInputVelocity( } if (*stop_seg_idx == output_traj_points.size() - 2) { const double signed_projected_length_to_segment = - autoware_motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( output_traj_points, *stop_seg_idx, input_stop_pose.position); - const double segment_length = autoware_motion_utils::calcSignedArcLength( + 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. diff --git a/planning/autoware_path_smoother/src/replan_checker.cpp b/planning/autoware_path_smoother/src/replan_checker.cpp index 67096d29cab8d..5d2e7a05e4b17 100644 --- a/planning/autoware_path_smoother/src/replan_checker.cpp +++ b/planning/autoware_path_smoother/src/replan_checker.cpp @@ -40,7 +40,7 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "replan.enable", enable_); updateParam( @@ -80,7 +80,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // ego pose is lost or new ego pose is designated in simulation const double delta_dist = - autoware_universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + autoware::universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -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 = autoware_motion_utils::calcLateralOffset( + 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 = - autoware_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,7 +170,7 @@ 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 = autoware_motion_utils::calcLongitudinalOffsetPoint( + const auto prev_forward_point = autoware::motion_utils::calcLongitudinalOffsetPoint( prev_traj_points, prev_ego_seg_idx, lon_dist); if (!prev_forward_point) { continue; @@ -178,9 +178,9 @@ bool ReplanChecker::isPathForwardChanged( // calculate lateral offset of current trajectory points to prev forward point const auto forward_seg_idx = - autoware_motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); - const double forward_lat_offset = - autoware_motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + autoware::motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = 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; } @@ -201,7 +201,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - autoware_universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware::universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index b55dc901b0cdc..4a36359ded4f3 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -54,10 +54,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = autoware_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 autoware_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 = autoware_motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = autoware_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 autoware_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 = autoware_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/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp index dfaefd7c882d1..9a93b647dac02 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -57,8 +57,8 @@ Pose createPoseFromLaneID(const lanelet::Id & lane_id) // calculate middle pose geometry_msgs::msg::Pose middle_pose; middle_pose.position = middle_pos; - const double yaw = autoware_universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return middle_pose; } 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 2907d264d1a1d..1526e061ca4d1 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 @@ -296,7 +296,7 @@ void PlanningInterfaceTestManager::publishNominalPath( { autoware::test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - autoware_motion_utils::convertToPath( + autoware::motion_utils::convertToPath( autoware::test_utils::loadPathWithLaneIdInYaml()), 5); } 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 a03897ebb86f6..ff2f3084fc9ba 100644 --- a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -24,7 +24,7 @@ namespace TrajectoryPoint convertToTrajectoryPoint(const PathPoint & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware_universe_utils::getPose(point); + traj_point.pose = autoware::universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -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 = autoware_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/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp index 1d47aefaa31e4..e996855b9b4da 100644 --- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp @@ -36,10 +36,10 @@ namespace autoware::planning_validator { +using autoware::universe_utils::StopWatch; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_planning_validator::msg::PlanningValidatorStatus; -using autoware_universe_utils::StopWatch; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; @@ -103,7 +103,7 @@ class PlanningValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - autoware_universe_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; @@ -137,9 +137,9 @@ class PlanningValidator : public rclcpp::Node std::shared_ptr debug_pose_publisher_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; StopWatch stop_watch_; }; diff --git a/planning/autoware_planning_validator/src/debug_marker.cpp b/planning/autoware_planning_validator/src/debug_marker.cpp index d4aa27ca2ee9a..7de23de8eda01 100644 --- a/planning/autoware_planning_validator/src/debug_marker.cpp +++ b/planning/autoware_planning_validator/src/debug_marker.cpp @@ -48,7 +48,7 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) { - using autoware_universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerColor; // append arrow marker std_msgs::msg::ColorRGBA color; @@ -64,9 +64,9 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( { color = createMarkerColor(0.0, 0.0, 1.0, 0.999); } - Marker marker = autoware_universe_utils::createDefaultMarker( + Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - autoware_universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); + autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; @@ -76,10 +76,10 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushWarningMsg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = autoware_universe_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - autoware_universe_utils::createMarkerScale(0.0, 0.0, 1.0), - autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -89,9 +89,9 @@ void PlanningValidatorDebugMarkerPublisher::pushWarningMsg( void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) { const auto now = node_->get_clock()->now(); - const auto stop_wall_marker = - autoware_motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); - autoware_universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + const auto stop_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + pose, "autoware_planning_validator", now, 0); + autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } void PlanningValidatorDebugMarkerPublisher::publish() diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp index b3f2ab88a4a45..29e8932c87842 100644 --- a/planning/autoware_planning_validator/src/planning_validator.cpp +++ b/planning/autoware_planning_validator/src/planning_validator.cpp @@ -44,9 +44,9 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void PlanningValidator::setupParameters() @@ -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 = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); validation_status_.velocity_deviation = std::abs( @@ -466,10 +466,10 @@ bool PlanningValidator::checkValidVelocityDeviation(const Trajectory & trajector bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajectory) { // TODO(horibe): set appropriate thresholds for index search - const auto idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); - validation_status_.distance_deviation = autoware_universe_utils::calcDistance2d( + validation_status_.distance_deviation = autoware::universe_utils::calcDistance2d( trajectory.points.at(idx), current_kinematics_->pose.pose); if (validation_status_.distance_deviation > validation_params_.distance_deviation_threshold) { @@ -487,7 +487,7 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory const auto ego_pose = current_kinematics_->pose.pose; const size_t idx = - autoware_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,13 +495,13 @@ 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 = autoware_motion_utils::calcLongitudinalOffsetToSegment( + 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) { const auto size = trajectory.points.size(); - long_offset -= autoware_universe_utils::calcDistance2d( + long_offset -= autoware::universe_utils::calcDistance2d( trajectory.points.at(size - 1), trajectory.points.at(size - 2)); } @@ -532,7 +532,7 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra return true; // Ego is almost stopped. } - const auto forward_length = autoware_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 3d21c89704a03..ae580a7a7c6f1 100644 --- a/planning/autoware_planning_validator/src/utils.cpp +++ b/planning/autoware_planning_validator/src/utils.cpp @@ -24,9 +24,9 @@ namespace autoware::planning_validator { -using autoware_universe_utils::calcCurvature; -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::getPoint; +using autoware::universe_utils::calcCurvature; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::getPoint; namespace { @@ -138,7 +138,7 @@ void calcCurvature( const auto p2 = getPoint(trajectory.points.at(i)); const auto p3 = getPoint(trajectory.points.at(next_idx)); try { - curvature_arr.at(i) = autoware_universe_utils::calcCurvature(p1, p2, p3); + curvature_arr.at(i) = autoware::universe_utils::calcCurvature(p1, p2, p3); } catch (...) { curvature_arr.at(i) = 0.0; // maybe distance is too close } @@ -236,12 +236,12 @@ std::pair calcMaxRelativeAngles(const Trajectory & trajectory) const auto & p2 = trajectory.points.at(i + 1).pose.position; const auto & p3 = trajectory.points.at(i + 2).pose.position; - const auto angle_a = autoware_universe_utils::calcAzimuthAngle(p1, p2); - const auto angle_b = autoware_universe_utils::calcAzimuthAngle(p2, p3); + const auto angle_a = autoware::universe_utils::calcAzimuthAngle(p1, p2); + const auto angle_b = autoware::universe_utils::calcAzimuthAngle(p2, p3); // convert relative angle to [-pi ~ pi] const auto relative_angle = - std::abs(autoware_universe_utils::normalizeRadian(angle_b - angle_a)); + std::abs(autoware::universe_utils::normalizeRadian(angle_b - angle_a)); takeBigger(max_relative_angles, max_index, std::abs(relative_angle), i); } diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp index b4985700fdcb6..bdf4ca96d9fdd 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp @@ -19,9 +19,9 @@ #include +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::createQuaternionFromYaw; Trajectory generateTrajectoryWithConstantAcceleration( const double interval_distance, const double speed, const double yaw, const size_t size, diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 4c1eb1d1c4ef6..da1292fa862c6 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -143,7 +143,7 @@ void RemainingDistanceTimeCalculatorNode::calculate_remaining_distance() for (auto & llt : remaining_shortest_path) { if (remaining_shortest_path.size() == 1) { - remaining_distance_ += autoware_universe_utils::calcDistance2d( + remaining_distance_ += autoware::universe_utils::calcDistance2d( current_vehicle_pose_.position, goal_pose_.position); break; } diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index cffac44b6947e..d534ecc4c84a8 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -110,7 +110,7 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) constexpr double min_dist = 0.001; if ( - autoware_universe_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < + autoware::universe_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < min_dist) { filtered_path.points.back().lane_ids.push_back(pt.lane_ids.front()); filtered_path.points.back().point.longitudinal_velocity_mps = std::min( @@ -1725,14 +1725,14 @@ PathWithLaneId RouteHandler::getCenterLinePath( double angle{0.0}; const auto & pts = reference_path.points; if (i + 1 < reference_path.points.size()) { - angle = autoware_universe_utils::calcAzimuthAngle( + angle = autoware::universe_utils::calcAzimuthAngle( pts.at(i).point.pose.position, pts.at(i + 1).point.pose.position); } else if (i != 0) { - angle = autoware_universe_utils::calcAzimuthAngle( + angle = autoware::universe_utils::calcAzimuthAngle( pts.at(i - 1).point.pose.position, pts.at(i).point.pose.position); } reference_path.points.at(i).point.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(angle); + autoware::universe_utils::createQuaternionFromYaw(angle); } return reference_path; diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp index 056d46056f452..35a2fe3ef45da 100644 --- a/planning/autoware_route_handler/test/test_route_handler.cpp +++ b/planning/autoware_route_handler/test/test_route_handler.cpp @@ -53,10 +53,10 @@ TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) ASSERT_FALSE(route_handler_->isHandlerReady()); geometry_msgs::msg::Pose start_pose, goal_pose; - start_pose.position = autoware_universe_utils::createPoint(3728.870361, 73739.281250, 0); - start_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, -0.513117, 0.858319); - goal_pose.position = autoware_universe_utils::createPoint(3729.961182, 73727.328125, 0); - goal_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, 0.234831, 0.972036); + start_pose.position = autoware::universe_utils::createPoint(3728.870361, 73739.281250, 0); + start_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, -0.513117, 0.858319); + goal_pose.position = autoware::universe_utils::createPoint(3729.961182, 73727.328125, 0); + goal_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, 0.234831, 0.972036); lanelet::ConstLanelets path_lanelets; ASSERT_TRUE( @@ -86,16 +86,17 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) geometry_msgs::msg::Pose reference_pose, search_pose; lanelet::ConstLanelet reference_lanelet; - reference_pose.position = autoware_universe_utils::createPoint(3730.88, 73735.3, 0); - reference_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, -0.504626, 0.863338); + reference_pose.position = autoware::universe_utils::createPoint(3730.88, 73735.3, 0); + reference_pose.orientation = + autoware::universe_utils::createQuaternion(0, 0, -0.504626, 0.863338); const auto found_reference_lanelet = route_handler_->getClosestLaneletWithinRoute(reference_pose, &reference_lanelet); ASSERT_TRUE(found_reference_lanelet); ASSERT_EQ(reference_lanelet.id(), 168); lanelet::ConstLanelet closest_lanelet; - search_pose.position = autoware_universe_utils::createPoint(3736.89, 73730.8, 0); - search_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, 0.223244, 0.974763); + search_pose.position = autoware::universe_utils::createPoint(3736.89, 73730.8, 0); + search_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, 0.223244, 0.974763); bool found_lanelet = route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lanelet); ASSERT_TRUE(found_lanelet); ASSERT_EQ(closest_lanelet.id(), 345); @@ -112,56 +113,56 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) // Pose search_pose; -// search_pose.position = autoware_universe_utils::createPoint(-1.0, 1.75, 0); -// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(-1.0, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained7 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained7); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = autoware_universe_utils::createPoint(-0.5, 1.75, 0); +// search_pose.position = autoware::universe_utils::createPoint(-0.5, 1.75, 0); // const auto closest_lane_obtained = -// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = autoware_universe_utils::createPoint(-.01, 1.75, 0); -// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(-.01, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained3 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained3); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = autoware_universe_utils::createPoint(0.0, 1.75, 0); -// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(0.0, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained1 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained1); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = autoware_universe_utils::createPoint(0.01, 1.75, 0); -// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(0.01, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained2 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained2); // ASSERT_EQ(closest_lane.id(), 4424); -// search_pose.position = autoware_universe_utils::createPoint(0.5, 1.75, 0); -// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(0.5, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained4 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained4); // ASSERT_EQ(closest_lane.id(), 4424); -// search_pose.position = autoware_universe_utils::createPoint(1.0, 1.75, 0); -// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(1.0, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained5 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index e008a3d647436..557c0c871583d 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -73,12 +73,12 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_route_; - autoware_universe_utils::InterProcessPollingSubscriber::SharedPtr + autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_lane_driving_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_trajectory_; - autoware_universe_utils::InterProcessPollingSubscriber::SharedPtr + autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr sub_parking_state_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; @@ -93,7 +93,7 @@ class ScenarioSelectorNode : public rclcpp::Node std::deque twist_buffer_; std::shared_ptr route_handler_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // Parameters double update_rate_; diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index d1516c68050c1..4dcf1b6adb111 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -382,7 +382,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index d73573833d1fa..cbfcac5789812 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -73,10 +73,10 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) centerline_traj_points.at(i - 1).pose.orientation; } } else { - const double yaw_angle = autoware_universe_utils::calcAzimuthAngle( + const double yaw_angle = autoware::universe_utils::calcAzimuthAngle( centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); centerline_traj_points.at(i).pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(yaw_angle); + autoware::universe_utils::createQuaternionFromYaw(yaw_angle); } } 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 7bba3a6d5a722..1f72dea1cd35f 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 @@ -82,20 +82,20 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( // get ego nearest search parameters and resample interval in behavior_path_planner const double ego_nearest_dist_threshold = - autoware_universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); const double ego_nearest_yaw_threshold = - autoware_universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); const double behavior_path_interval = - autoware_universe_utils::getOrDeclareParameter(node, "output_path_interval"); + autoware::universe_utils::getOrDeclareParameter(node, "output_path_interval"); const double behavior_vel_interval = - autoware_universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); + autoware::universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { 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 autoware_motion_utils::resamplePath( + 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); @@ -104,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 autoware_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."); @@ -124,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 autoware_motion_utils::convertToTrajectoryPointArray(raw_traj); + return autoware::motion_utils::convertToTrajectoryPointArray(raw_traj); }(); // create an instance of elastic band and model predictive trajectory. @@ -166,7 +166,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // connect the previously and currently optimized trajectory points for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = autoware_universe_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( whole_optimized_traj_points.at(j), optimized_traj_points.front()); if (dist < 0.5) { const std::vector extracted_whole_optimized_traj_points{ 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 706f0e9c75475..c5ecc48ee209d 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 @@ -89,13 +89,13 @@ LinearRing2d create_vehicle_footprint( std::vector geom_points; geom_points.push_back( - autoware_universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); geom_points.push_back( - autoware_universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); geom_points.push_back( - autoware_universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); geom_points.push_back( - autoware_universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); LinearRing2d footprint; for (const auto & geom_point : geom_points) { @@ -117,7 +117,7 @@ geometry_msgs::msg::Pose get_text_pose( const double x_front = i.front_overhang_m + i.wheel_base_m; const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 1.0; - return autoware_universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); + return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); } std::array convert_hex_string_to_decimal(const std::string & hex_str_color) @@ -166,10 +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 = autoware_motion_utils::convertToTrajectory(input_traj_points); + 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); + autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); } } // namespace @@ -203,7 +203,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( sub_save_map_ = create_subscription( "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { const auto lanelet2_output_file_path = - autoware_universe_utils::getOrDeclareParameter( + autoware::universe_utils::getOrDeclareParameter( *this, "lanelet2_output_file_path"); if (!centerline_with_route_ || msg.data) { const auto & c = *centerline_with_route_; @@ -274,7 +274,7 @@ void StaticCenterlineGeneratorNode::update_centerline_range( centerline.begin() + traj_range_indices_.second + 1); pub_centerline_->publish( - autoware_motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); + autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); } void StaticCenterlineGeneratorNode::run() @@ -326,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(autoware_motion_utils::convertToTrajectory( + pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); - pub_centerline_->publish(autoware_motion_utils::convertToTrajectory( + pub_centerline_->publish(autoware::motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); return centerline_with_route; @@ -577,10 +577,11 @@ void StaticCenterlineGeneratorNode::evaluate( const std::vector & optimized_traj_points) { const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - const auto dist_thresh_vec = autoware_universe_utils::getOrDeclareParameter>( + const auto dist_thresh_vec = autoware::universe_utils::getOrDeclareParameter>( *this, "marker_color_dist_thresh"); const auto marker_color_vec = - autoware_universe_utils::getOrDeclareParameter>(*this, "marker_color"); + autoware::universe_utils::getOrDeclareParameter>( + *this, "marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); @@ -627,13 +628,13 @@ void StaticCenterlineGeneratorNode::evaluate( // add footprint marker const auto footprint_marker = utils::create_footprint_marker(footprint_poly, marker_color, now(), i); - autoware_universe_utils::appendMarkerArray(footprint_marker, &marker_array); + autoware::universe_utils::appendMarkerArray(footprint_marker, &marker_array); // add text of distance to bounds marker const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); const auto text_marker = utils::create_distance_text_marker(text_pose, min_dist_to_bound, marker_color, now(), i); - autoware_universe_utils::appendMarkerArray(text_marker, &marker_array); + autoware::universe_utils::appendMarkerArray(text_marker, &marker_array); } } diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index 88b4d3ffa14b4..c5157acc2b525 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -29,6 +29,9 @@ namespace autoware::static_centerline_generator { using autoware::route_handler::RouteHandler; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; @@ -36,9 +39,6 @@ using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::LineString2d; -using autoware_universe_utils::Point2d; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index 10b2589f3bf7b..de362e3af5d7f 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -94,8 +94,8 @@ geometry_msgs::msg::Pose get_center_pose( // calculate middle pose geometry_msgs::msg::Pose middle_pose; middle_pose.position = middle_pos; - const double yaw = autoware_universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return middle_pose; } @@ -189,11 +189,11 @@ MarkerArray create_footprint_marker( const double g = marker_color.at(1); const double b = marker_color.at(2); - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints", idx, visualization_msgs::msg::Marker::LINE_STRIP, - autoware_universe_utils::createMarkerScale(0.1, 0.0, 0.0), - autoware_universe_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); @@ -221,11 +221,11 @@ MarkerArray create_distance_text_marker( const double g = marker_color.at(1); const double b = marker_color.at(2); - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware_universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware_universe_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), + autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index e4eaf259e95f9..7897a868ee172 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -52,12 +52,12 @@ Polygon2d createSelfPolygon( } } // namespace -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 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; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double base_link2front, diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 8a02c6d686736..9c899c2226497 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -51,9 +51,9 @@ namespace autoware::surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::pose2transform; using autoware_perception_msgs::msg::ObjectClassification; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::pose2transform; namespace { @@ -193,7 +193,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); @@ -251,12 +251,12 @@ rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( use_dynamic_object_ = false; for (const auto & label_pair : label_map_) { bool & check_current_label = node_param_.enable_check_map.at(label_pair.second); - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, label_pair.first + ".enable_check", check_current_label); use_dynamic_object_ = use_dynamic_object_ || check_current_label; } - autoware_universe_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "debug_footprint_label", node_param_.debug_footprint_label); const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); debug_ptr_->updateFootprintMargin( @@ -411,7 +411,7 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl tf2::transformToEigen(transform_stamped.value().transform).cast(); pcl::PointCloud transformed_pointcloud; pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( transformed_pointcloud, transformed_pointcloud, isometry); const double front_margin = node_param_.pointcloud_surround_check_front_distance; diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index 8a21f6cee6a32..e0bcf771948ca 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -46,8 +46,8 @@ namespace autoware::surround_obstacle_checker { +using autoware::motion_utils::VehicleStopChecker; using autoware::vehicle_info_utils::VehicleInfo; -using autoware_motion_utils::VehicleStopChecker; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using tier4_planning_msgs::msg::VelocityLimit; @@ -111,11 +111,11 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // publisher and subscriber - autoware_universe_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber - sub_pointcloud_{this, "~/input/pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; - autoware_universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ + autoware::universe_utils::InterProcessPollingSubscriber + sub_pointcloud_{this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; @@ -143,7 +143,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; bool use_dynamic_object_; 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 b64f681be46fe..0dd18615be5ef 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -88,13 +88,13 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; - autoware_universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_current_acceleration_{this, "~/input/acceleration"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/operation_mode_state"}; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry @@ -245,7 +245,7 @@ class VelocitySmootherNode : public rclcpp::Node void initCommonParam(); // debug - autoware_universe_utils::StopWatch stop_watch_; + autoware::universe_utils::StopWatch stop_watch_; std::shared_ptr prev_time_; double prev_acc_; rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; @@ -273,8 +273,8 @@ class VelocitySmootherNode : public rclcpp::Node void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); - std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 42c5f1657999c..aefbd9ef20f84 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -90,9 +90,9 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti clock_ = get_clock(); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void VelocitySmootherNode::setupSmoother(const double wheelbase) @@ -300,7 +300,7 @@ void VelocitySmootherNode::initCommonParam() void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { - Trajectory publishing_trajectory = autoware_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 = autoware_motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); + auto input_points = autoware::motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); // guard for invalid trajectory - input_points = autoware_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 = autoware_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,13 @@ void VelocitySmootherNode::insertBehindVelocity( } // with distance threshold - const auto opt_second_nearest_seg_idx = autoware_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 autoware_motion_utils::findNearestSegmentIndex( + return autoware::motion_utils::findNearestSegmentIndex( prev_output_, output.at(i).pose.position); }(); const auto prev_output_point = @@ -722,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{autoware_motion_utils::searchZeroVelocityIndex(trajectory)}; + const auto stop_idx{autoware::motion_utils::searchZeroVelocityIndex(trajectory)}; if (stop_idx) { - stop_dist = autoware_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; } @@ -820,14 +820,14 @@ std::pair VelocitySmootherNode::ca void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { - const auto stop_idx = autoware_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 = autoware_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); @@ -876,11 +876,11 @@ 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 = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj, current_pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); const auto inserted_index = - autoware_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)); @@ -893,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 = autoware_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); @@ -905,13 +905,13 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { - const auto stop_idx = autoware_motion_utils::searchZeroVelocityIndex(traj); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. } double distance_sum = 0.0; for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward - distance_sum += autoware_universe_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); + distance_sum += autoware::universe_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); if (distance_sum > node_param_.stopping_distance) { break; } @@ -1022,7 +1022,7 @@ double VelocitySmootherNode::calcTravelDistance() const if (prev_closest_point_) { const double travel_dist = - autoware_universe_utils::calcDistance2d(*prev_closest_point_, closest_point); + autoware::universe_utils::calcDistance2d(*prev_closest_point_, closest_point); return travel_dist; } @@ -1039,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 = autoware_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 autoware_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); } @@ -1077,7 +1077,7 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( const TrajectoryPoints & trajectory, const Pose & pose) const { const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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 f24dc17df0538..f69ad5a592272 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -33,17 +33,17 @@ TrajectoryPoints resampleTrajectory( { // Arc length from the initial point to the closest point const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double negative_front_arclength_value = autoware_motion_utils::calcSignedArcLength( + 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 = - autoware_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 = autoware_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); @@ -128,14 +128,14 @@ TrajectoryPoints resampleTrajectory( return input; } - 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); + 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) { constexpr double ep_dist = 1.0E-3; - if (autoware_universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); @@ -151,9 +151,9 @@ TrajectoryPoints resampleTrajectory( const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) { // input arclength - const double trajectory_length = autoware_motion_utils::calcArcLength(input); + const double trajectory_length = autoware::motion_utils::calcArcLength(input); const auto dist_to_closest_stop_point = - autoware_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; @@ -172,9 +172,9 @@ TrajectoryPoints resampleTrajectory( // Step1. Resample front trajectory // Arc length from the initial point to the closest point const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double negative_front_arclength_value = autoware_motion_utils::calcSignedArcLength( + 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); @@ -248,14 +248,14 @@ TrajectoryPoints resampleTrajectory( return input; } - 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); + 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) { constexpr double ep_dist = 1.0E-3; - if (autoware_universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); 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 27b54ccf8bf2b..7f263fdea5e36 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 @@ -252,7 +252,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( const auto tp1 = input.at(i + 1); const double dist_thr = 0.001; // 1mm - const double dist_tp0_tp1 = autoware_universe_utils::calcDistance2d(tp0, tp1); + const double dist_tp0_tp1 = autoware::universe_utils::calcDistance2d(tp0, tp1); if (std::fabs(dist_tp0_tp1) < dist_thr) { output.push_back(input.at(i)); continue; @@ -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 = autoware_motion_utils::resampleTrajectory( - autoware_motion_utils::convertToTrajectory(input), out_arclength); - output = autoware_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; @@ -355,7 +355,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt } if ( - autoware_universe_utils::calcDistance2d(output.at(end_index), output.at(index)) < + autoware::universe_utils::calcDistance2d(output.at(end_index), output.at(index)) < dist_threshold) { end_index = index; min_latacc_velocity = std::min( @@ -441,7 +441,7 @@ bool AnalyticalJerkConstrainedSmoother::applyForwardJerkFilter( for (size_t i = start_index + 1; i < base_trajectory.size(); ++i) { const double prev_vel = output_trajectory.at(i - 1).longitudinal_velocity_mps; const double ds = - autoware_universe_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); + autoware::universe_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); const double dt = ds / std::max(prev_vel, 1.0); const double prev_acc = output_trajectory.at(i - 1).acceleration_mps2; @@ -487,7 +487,7 @@ bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter( } } for (size_t i = decel_target_index; i > start_index; --i) { - dist += autoware_universe_utils::calcDistance2d( + dist += autoware::universe_utils::calcDistance2d( output_trajectory.at(i - 1), output_trajectory.at(i)); dist_to_target.at(i - 1) = dist; } diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index fd9c4f34b4dc7..739ba6e7cef38 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -225,8 +225,8 @@ bool calcStopVelocityWithConstantJerkAccLimit( std::vector distances; distances.push_back(distance); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { - distance += - autoware_universe_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); + distance += autoware::universe_utils::calcDistance2d( + output_trajectory.at(i), output_trajectory.at(i + 1)); if (distance > xs.back()) { break; } 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 9f9f50b955757..cab8e49a3b45d 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 = autoware_motion_utils::searchZeroVelocityIndex( + const auto zero_vel_id = autoware::motion_utils::searchZeroVelocityIndex( opt_resampled_trajectory, 1, opt_resampled_trajectory.size()); if (!zero_vel_id) { @@ -385,7 +385,7 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( output.front().longitudinal_velocity_mps = current_vel; output.front().acceleration_mps2 = current_acc; for (size_t i = 1; i < input.size(); ++i) { - const double ds = autoware_universe_utils::calcDistance2d(input.at(i), input.at(i - 1)); + const double ds = autoware::universe_utils::calcDistance2d(input.at(i), input.at(i - 1)); const double max_dt = std::pow(6.0 * ds / j_max, 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); @@ -439,8 +439,8 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( merged.at(i).longitudinal_velocity_mps = current_vel; merged.at(i).acceleration_mps2 = current_acc; - const double ds = - autoware_universe_utils::calcDistance2d(forward_filtered.at(i + 1), forward_filtered.at(i)); + const double ds = autoware::universe_utils::calcDistance2d( + forward_filtered.at(i + 1), forward_filtered.at(i)); const double max_dt = std::pow(6.0 * ds / std::fabs(j_min), 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index a1972d5dbc1e0..700cf45b7eb9d 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -34,10 +34,10 @@ namespace TrajectoryPoints applyPreProcess( const TrajectoryPoints & input, const double interval, const bool use_resampling) { - using autoware_motion_utils::calcArcLength; - using autoware_motion_utils::convertToTrajectory; - using autoware_motion_utils::convertToTrajectoryPointArray; - using autoware_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 = autoware_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 = autoware_motion_utils::resampleTrajectory( - autoware_motion_utils::convertToTrajectory(input), out_arclength); - output = autoware_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; @@ -249,7 +249,7 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( } const auto steer_rate = steer_rate_arr.at(i); - if (steer_rate < autoware_universe_utils::deg2rad(base_param_.max_steering_angle_rate)) { + if (steer_rate < autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate)) { continue; } @@ -257,7 +257,7 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; const auto target_mean_vel = mean_vel * - (autoware_universe_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); + (autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); for (size_t k = 0; k < 2; k++) { auto & velocity = output.at(i + k).longitudinal_velocity_mps; diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index 51a2be57ebe02..87ad64d3ff976 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -34,7 +34,7 @@ inline void convertEulerAngleToMonotonic(std::vector & a) { for (unsigned int i = 1; i < a.size(); ++i) { const double da = a[i] - a[i - 1]; - a[i] = a[i - 1] + autoware_universe_utils::normalizeRadian(da); + a[i] = a[i - 1] + autoware::universe_utils::normalizeRadian(da); } } @@ -87,7 +87,7 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( { const auto & seg_pt = trajectory.at(seg_idx); const auto & next_pt = trajectory.at(seg_idx + 1); - traj_p.pose = autoware_universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); + traj_p.pose = autoware::universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); traj_p.longitudinal_velocity_mps = interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = @@ -110,7 +110,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum = 0.0; for (size_t i = index; i < trajectory.size() - 1; ++i) { - dist_sum += autoware_universe_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); + dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); if (dist_sum > ahead_length) { ahead_index = i + 1; break; @@ -123,7 +123,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum{0.0}; for (size_t i = index; i != 0; --i) { - dist_sum += autoware_universe_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); + dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); if (dist_sum > behind_length) { behind_index = i - 1; break; @@ -152,7 +152,7 @@ std::vector calcArclengthArray(const TrajectoryPoints & trajectory) for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - dist += autoware_universe_utils::calcDistance2d(tp.pose, tp_prev.pose); + dist += autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); arclength.at(i) = dist; } return arclength; @@ -164,7 +164,7 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - const double dist = autoware_universe_utils::calcDistance2d(tp.pose, tp_prev.pose); + const double dist = autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); intervals.push_back(dist); } return intervals; @@ -173,8 +173,8 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using autoware_universe_utils::calcCurvature; - using autoware_universe_utils::getPoint; + using autoware::universe_utils::calcCurvature; + using autoware::universe_utils::getPoint; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); @@ -596,7 +596,7 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest) { - const auto idx = autoware_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 @@ -604,7 +604,7 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes // TODO(Horibe): use arc length distance const double stop_dist = - autoware_universe_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); + autoware::universe_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); return stop_dist; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 267963dc32681..02b90186d9b2f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -32,8 +32,8 @@ using autoware::behavior_path_planner::ObjectParameter; void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { + using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; - using autoware_universe_utils::getOrDeclareParameter; // init manager interface initInterface(node, {"left", "right"}); 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 247f94c63fc8f..a19cbda6ccad7 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,9 +195,9 @@ 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 autoware::motion_utils::findNearestIndex; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::calcLateralDeviation; using boost::geometry::return_centroid; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 53ecf19b8215f..6ea15a9309c3c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -57,8 +57,8 @@ std::vector getAllKeys(const std::unordered_map & map) namespace autoware::behavior_path_planner { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedPath; -using autoware_universe_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; struct MinMaxValue @@ -176,7 +176,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, const bool arg_is_object_on_ego_path, const std::optional & arg_latest_time_inside_ego_path) - : uuid(autoware_universe_utils::toHexString(predicted_object.object_id)), + : uuid(autoware::universe_utils::toHexString(predicted_object.object_id)), label(predicted_object.classification.front().label), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), shape(predicted_object.shape), @@ -374,8 +374,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface }; struct EgoPathReservePoly { - const autoware_universe_utils::Polygon2d left_avoid; - const autoware_universe_utils::Polygon2d right_avoid; + const autoware::universe_utils::Polygon2d left_avoid; + const autoware::universe_utils::Polygon2d right_avoid; }; bool canTransitSuccessState() override; @@ -431,11 +431,11 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const DynamicAvoidanceObject & object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; - std::optional calcEgoPathBasedDynamicObstaclePolygon( + std::optional calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcObjectPathBasedDynamicObstaclePolygon( + std::optional calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcPredictedPathBasedDynamicObstaclePolygon( + std::optional calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const; EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const; @@ -455,7 +455,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface TargetObjectsManager target_objects_manager_; - mutable autoware_universe_utils::StopWatch< + mutable autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index 9b0db95bb0df5..eccf2b2a1bfde 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp @@ -152,7 +152,7 @@ void DynamicObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void DynamicObstacleAvoidanceModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; { // common 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 0c8ec1c530b7a..8b411340e4ddc 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 @@ -41,7 +41,7 @@ namespace autoware::behavior_path_planner { namespace { -geometry_msgs::msg::Point toGeometryPoint(const autoware_universe_utils::Point2d & point) +geometry_msgs::msg::Point toGeometryPoint(const autoware::universe_utils::Point2d & point) { geometry_msgs::msg::Point geom_obj_point; geom_obj_point.x = point.x(); @@ -65,25 +65,25 @@ MinMaxValue combineMinMaxValues(const MinMaxValue & r1, const MinMaxValue & r2) void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) { - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, - autoware_universe_utils::createMarkerScale(3.0, 1.0, 1.0), - autoware_universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + autoware::universe_utils::createMarkerScale(3.0, 1.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); marker.pose = obj_pose; marker_array.markers.push_back(marker); } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const autoware_universe_utils::Polygon2d & obj_poly, + MarkerArray & marker_array, const autoware::universe_utils::Polygon2d & obj_poly, const double obj_z) { - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, - autoware_universe_utils::createMarkerScale(0.1, 0.0, 0.0), - autoware_universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. for (size_t i = 0; i < obj_poly.outer().size(); ++i) { @@ -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 = autoware_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); @@ -173,19 +173,19 @@ double calcDiffAngleAgainstPath( const geometry_msgs::msg::Pose & target_pose) { const size_t nearest_idx = - autoware_motion_utils::findNearestIndex(path_points, target_pose.position); + 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); - const double diff_yaw = autoware_universe_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); return diff_yaw; } [[maybe_unused]] double calcDiffAngleBetweenPaths( const std::vector & path_points, const PredictedPath & predicted_path) { - const size_t nearest_idx = autoware_motion_utils::findNearestSegmentIndex( + 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); @@ -193,7 +193,7 @@ double calcDiffAngleAgainstPath( double signed_max_angle{0.0}; for (size_t i = 0; i < std::min(max_predicted_path_size, predicted_path.path.size()); ++i) { const double obj_yaw = tf2::getYaw(predicted_path.path.at(i).orientation); - const double diff_yaw = autoware_universe_utils::normalizeRadian(obj_yaw - ego_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(obj_yaw - ego_yaw); if (std::abs(signed_max_angle) < std::abs(diff_yaw)) { signed_max_angle = diff_yaw; } @@ -205,34 +205,34 @@ double calcDistanceToPath( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) { - const size_t target_idx = autoware_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( + const double angle_to_target_pos = autoware::universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); const double diff_yaw = - autoware_universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); + autoware::universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); if ( (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || (target_idx == path_points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { - return autoware_universe_utils::calcDistance2d(path_points.at(target_idx), target_pos); + return autoware::universe_utils::calcDistance2d(path_points.at(target_idx), target_pos); } } - return std::abs(autoware_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 = autoware_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( + const double angle_to_target_pos = autoware::universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); const double diff_yaw = - autoware_universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); + autoware::universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); if (0 < diff_yaw) { return true; @@ -283,7 +283,7 @@ std::optional> intersectLines( ++source_seg_idx) { for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; ++target_seg_idx) { - const auto intersect_point = autoware_universe_utils::intersect( + const auto intersect_point = autoware::universe_utils::intersect( source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); if (intersect_point) { @@ -329,7 +329,7 @@ bool DynamicObstacleAvoidanceModule::isExecutionRequested() const } // check if the ego is driving forward - const auto is_driving_forward = autoware_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; } @@ -498,7 +498,7 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = autoware_universe_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware::universe_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -586,7 +586,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = autoware_universe_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware::universe_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -626,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(autoware_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; @@ -766,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 = autoware_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( @@ -788,7 +788,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by // "ego_path_base" - const auto obj_points = autoware_universe_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); @@ -831,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 = autoware_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( @@ -891,19 +891,19 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( const double y = feasible_lat_offset; const auto feasible_left_bound_point = - autoware_universe_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); const auto feasible_right_bound_point = - autoware_universe_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); } - autoware_universe_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), &debug_marker_); - autoware_universe_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), &debug_marker_); @@ -918,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(autoware_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) { @@ -927,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(autoware_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) { @@ -947,7 +947,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( std::optional collision_start_idx{std::nullopt}; double lon_dist = 0.0; for (size_t i = ego_idx; i < ego_path.size() - 1; ++i) { - lon_dist += autoware_universe_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); + lon_dist += autoware::universe_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); const double elapsed_time = lon_dist / ego_vel; const auto future_ego_pose = ego_path.at(i); @@ -956,7 +956,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( if (future_obj_pose) { const double dist_ego_to_obj = - autoware_universe_utils::calcDistance2d(future_ego_pose, *future_obj_pose); + autoware::universe_utils::calcDistance2d(future_ego_pose, *future_obj_pose); if (dist_ego_to_obj < 1.0) { if (!collision_start_idx) { collision_start_idx = i; @@ -982,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 = autoware_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; @@ -1054,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 = - autoware_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; } @@ -1070,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 = - autoware_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 ( @@ -1134,7 +1134,7 @@ 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( - autoware_motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); + 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; @@ -1190,8 +1190,8 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const autoware_perception_msgs::msg::Shape & obj_shape) const { 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); + 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. std::vector obj_lat_offset_vec; @@ -1199,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 = - autoware_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 = - autoware_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 = - autoware_motion_utils::calcLongitudinalOffsetToSegment(ego_path, obj_seg_idx, geom_obj_point); + const double lon_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( + ego_path, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -1226,15 +1226,15 @@ MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const { - const size_t obj_seg_idx = - autoware_motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); + const size_t obj_seg_idx = 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 = autoware_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); } @@ -1299,7 +1299,7 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( { const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = - autoware_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 = @@ -1317,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 autoware_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. @@ -1371,18 +1371,18 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx); const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx); if (prev_min_dist && next_min_dist) { - const double segment_length = autoware_universe_utils::calcDistance2d( + const double segment_length = autoware::universe_utils::calcDistance2d( obj_path.path.at(prev_valid_obj_path_end_idx), obj_path.path.at(next_valid_obj_path_end_idx)); const double partial_segment_length = segment_length * (dist_threshold_paths - *prev_min_dist) / (*next_min_dist - *prev_min_dist); - return autoware_motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( obj_path.path, 0, prev_valid_obj_path_end_idx) + partial_segment_length; } } - return autoware_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 @@ -1400,8 +1400,8 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( return true; } const size_t obj_point_idx = - autoware_motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); - const double paths_lat_diff = std::abs(autoware_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)); @@ -1419,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 = autoware_motion_utils::findNearestSegmentIndex( + 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( + 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); } @@ -1494,8 +1494,8 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( return true; } const size_t obj_point_idx = - autoware_motion_utils::findNearestIndex(ref_path_points_for_obj_poly, object.pose.position); - const double paths_lat_diff = std::abs(autoware_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)); @@ -1509,13 +1509,13 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( }(); const auto obj_occupancy_region = [&]() { - const auto obj_points = autoware_universe_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); 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 = autoware_motion_utils::calcLateralOffset( + const double obj_point_lat_offset = autoware::motion_utils::calcLateralOffset( ref_path_points_for_obj_poly, geom_obj_point, - autoware_motion_utils::findNearestSegmentIndex( + autoware::motion_utils::findNearestSegmentIndex( ref_path_points_for_obj_poly, geom_obj_point)); lat_pos_vec.push_back(obj_point_lat_offset); } @@ -1566,7 +1566,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( } // NOTE: object does not have const only to update min_bound_lat_offset. -std::optional +std::optional DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1576,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 = autoware_motion_utils::findNearestSegmentIndex( + 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 obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); - const auto lon_bound_start_idx_opt = autoware_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 = autoware_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) { @@ -1602,7 +1602,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. - obj_inner_bound_poses.push_back(autoware_universe_utils::calcOffsetPose( + obj_inner_bound_poses.push_back(autoware::universe_utils::calcOffsetPose( ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, 0.0)); } @@ -1618,18 +1618,18 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( // Check if the object polygon intersects with the ego_lat_feasible_path. if (intersect_result) { const auto & [bound_seg_idx, intersect_point] = *intersect_result; - const double lon_offset = autoware_universe_utils::calcDistance2d( + const double lon_offset = autoware::universe_utils::calcDistance2d( obj_inner_bound_poses.at(bound_seg_idx), intersect_point); const auto obj_inner_bound_start_idx_opt = - autoware_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 = autoware_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) || @@ -1650,17 +1650,17 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector feasible_obj_outer_bound_points; for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { feasible_obj_outer_bound_points.push_back( - autoware_universe_utils::calcOffsetPose( + autoware::universe_utils::calcOffsetPose( feasible_obj_inner_bound_pose, 0.0, object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) .position); } // create obj_polygon from inner/outer bound points - autoware_universe_utils::Polygon2d obj_poly; + autoware::universe_utils::Polygon2d obj_poly; const auto add_points_to_obj_poly = [&](const auto & bound_points) { for (const auto & bound_point : bound_points) { - obj_poly.outer().push_back(autoware_universe_utils::Point2d(bound_point.x, bound_point.y)); + obj_poly.outer().push_back(autoware::universe_utils::Point2d(bound_point.x, bound_point.y)); } }; add_points_to_obj_poly(feasible_obj_inner_bound_points); @@ -1672,7 +1672,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( } // should be replace by the function calcPredictedPathBasedDynamicObstaclePolygon() (takagi) -std::optional +std::optional DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1683,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 = autoware_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) @@ -1701,26 +1701,26 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const auto & obj_pose = obj_path.path.at(i); obj_left_bound_points.push_back( - autoware_universe_utils::calcOffsetPose( + autoware::universe_utils::calcOffsetPose( obj_pose, lon_offset, object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0) .position); obj_right_bound_points.push_back( - autoware_universe_utils::calcOffsetPose( + autoware::universe_utils::calcOffsetPose( obj_pose, lon_offset, -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 0.0) .position); } // create obj_polygon from inner/outer bound points - autoware_universe_utils::Polygon2d obj_poly; + autoware::universe_utils::Polygon2d obj_poly; for (const auto & bound_point : obj_right_bound_points) { - const auto obj_poly_point = autoware_universe_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware::universe_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end()); for (const auto & bound_point : obj_left_bound_points) { - const auto obj_poly_point = autoware_universe_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware::universe_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } @@ -1731,7 +1731,7 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // Calculate polygons according to predicted_path with certain confidence, // except for the area required for ego safety. // input: an object, the minimum area required for ego safety, and some global params -std::optional +std::optional DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const { @@ -1747,10 +1747,10 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( } } - autoware_universe_utils::Polygon2d obj_points_as_poly; + autoware::universe_utils::Polygon2d obj_points_as_poly; for (const auto pose : obj_poses) { boost::geometry::append( - obj_points_as_poly, autoware_universe_utils::toFootprint( + obj_points_as_poly, autoware::universe_utils::toFootprint( pose, object.shape.dimensions.x * 0.5, object.shape.dimensions.x * 0.5, object.shape.dimensions.y * 0.5) .outer()); @@ -1759,7 +1759,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( Polygon2d obj_poly; boost::geometry::convex_hull(obj_points_as_poly, obj_poly); - autoware_universe_utils::MultiPolygon2d expanded_poly; + autoware::universe_utils::MultiPolygon2d expanded_poly; namespace strategy = boost::geometry::strategy::buffer; boost::geometry::buffer( obj_poly, expanded_poly, @@ -1768,7 +1768,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( strategy::point_circle()); if (expanded_poly.empty()) return {}; - autoware_universe_utils::MultiPolygon2d output_poly; + autoware::universe_utils::MultiPolygon2d output_poly; boost::geometry::difference( expanded_poly[0], object.is_collision_left ? ego_path_poly.right_avoid : ego_path_poly.left_avoid, output_poly); @@ -1804,27 +1804,27 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg assert(!ego_path.points.empty()); - autoware_universe_utils::LineString2d ego_path_lines; + autoware::universe_utils::LineString2d ego_path_lines; for (const auto & path_point : ego_path.points) { ego_path_lines.push_back( - autoware_universe_utils::fromMsg(path_point.point.pose.position).to_2d()); + autoware::universe_utils::fromMsg(path_point.point.pose.position).to_2d()); } auto calcReservePoly = [&ego_path_lines]( const strategy::distance_asymmetric path_expand_strategy, const strategy::distance_asymmetric steer_expand_strategy, const std::vector & outer_body_path) - -> autoware_universe_utils::Polygon2d { + -> autoware::universe_utils::Polygon2d { // reserve area based on the reference path - autoware_universe_utils::MultiPolygon2d path_poly; + autoware::universe_utils::MultiPolygon2d path_poly; boost::geometry::buffer( ego_path_lines, path_poly, path_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); // reserve area steer to the avoidance path - autoware_universe_utils::LineString2d steer_lines; + autoware::universe_utils::LineString2d steer_lines; for (const auto & point : outer_body_path) { - const auto bg_point = autoware_universe_utils::fromMsg(point).to_2d(); + const auto bg_point = autoware::universe_utils::fromMsg(point).to_2d(); if (boost::geometry::within(bg_point, path_poly)) { if (steer_lines.size() != 0) { ; @@ -1834,12 +1834,12 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg } // boost::geometry::append(steer_lines, bg_point); } - autoware_universe_utils::MultiPolygon2d steer_poly; + autoware::universe_utils::MultiPolygon2d steer_poly; boost::geometry::buffer( steer_lines, steer_poly, steer_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); - autoware_universe_utils::MultiPolygon2d output_poly; + autoware::universe_utils::MultiPolygon2d output_poly; boost::geometry::union_(path_poly, steer_poly, output_poly); if (output_poly.size() != 1) { assert(false); @@ -1853,11 +1853,11 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg const double vehicle_half_width = planner_data_->parameters.vehicle_width * 0.5; const double reserve_width_obj_side = vehicle_half_width - parameters_->max_lat_offset_to_avoid; - const autoware_universe_utils::Polygon2d left_avoid_poly = calcReservePoly( + const autoware::universe_utils::Polygon2d left_avoid_poly = calcReservePoly( strategy::distance_asymmetric(vehicle_half_width, reserve_width_obj_side), strategy::distance_asymmetric(vehicle_half_width, 0.0), motion_saturated_outer_paths.right_path); - const autoware_universe_utils::Polygon2d right_avoid_poly = calcReservePoly( + const autoware::universe_utils::Polygon2d right_avoid_poly = calcReservePoly( strategy::distance_asymmetric(reserve_width_obj_side, vehicle_half_width), strategy::distance_asymmetric(0.0, vehicle_half_width), motion_saturated_outer_paths.left_path); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index dc8a60c9a8373..f670e3b05fa77 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -22,7 +22,7 @@ #include -using autoware_universe_utils::LinearRing2d; +using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; 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 ed2735448fdbc..e34cd202011d0 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 @@ -69,7 +69,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::Polygon2d; #define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ public: \ @@ -387,7 +387,7 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr ego_predicted_path_params; std::shared_ptr objects_filtering_params; std::shared_ptr safety_check_params; - autoware_universe_utils::LinearRing2d vehicle_footprint; + autoware::universe_utils::LinearRing2d vehicle_footprint; PlannerData planner_data; ModuleStatus current_status; @@ -409,7 +409,7 @@ class GoalPlannerModule : public SceneModuleInterface const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, const std::shared_ptr goal_searcher_, - const autoware_universe_utils::LinearRing2d & vehicle_footprint); + const autoware::universe_utils::LinearRing2d & vehicle_footprint); private: void initializeOccupancyGridMap( @@ -484,7 +484,7 @@ class GoalPlannerModule : public SceneModuleInterface std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; - autoware_universe_utils::LinearRing2d vehicle_footprint_; + autoware::universe_utils::LinearRing2d vehicle_footprint_; std::recursive_mutex mutex_; // TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 38c55b0b87ecb..5d056add4665e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -23,7 +23,7 @@ namespace autoware::behavior_path_planner { -using autoware_universe_utils::LinearRing2d; +using autoware::universe_utils::LinearRing2d; using BasicPolygons2d = std::vector; class GoalSearcher : public GoalSearcherBase diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp index 4a1a925eb114c..baead4c229efd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -26,7 +26,7 @@ namespace autoware::behavior_path_planner { -using autoware_universe_utils::MultiPolygon2d; +using autoware::universe_utils::MultiPolygon2d; using geometry_msgs::msg::Pose; struct GoalCandidate 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 d1760c7f5a137..d2ba416c7fa90 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 @@ -25,7 +25,7 @@ #include #include -using autoware_universe_utils::LinearRing2d; +using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -67,7 +67,7 @@ struct PullOverPath partial_paths.at(i).points.end()); } } - path.points = autoware_motion_utils::removeOverlapPoints(path.points); + path.points = autoware::motion_utils::removeOverlapPoints(path.points); return path; } @@ -76,7 +76,7 @@ struct PullOverPath { const PathWithLaneId full_path = getFullPath(); const size_t start_idx = - autoware_motion_utils::findNearestIndex(full_path.points, start_pose.position); + 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/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 927145fc69db1..98a513d519b43 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -42,7 +42,7 @@ using geometry_msgs::msg::Twist; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using Polygon2d = autoware_universe_utils::Polygon2d; +using Polygon2d = autoware::universe_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -94,8 +94,8 @@ std::vector createPathFootPrints( // debug MarkerArray createPullOverAreaMarkerArray( - const autoware_universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, - const std_msgs::msg::ColorRGBA & color, const double z); + const autoware::universe_utils::MultiPolygon2d area_polygons, + const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z); MarkerArray createPosesMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createTextsMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index b5dac4264de39..10e0137978bd6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -25,7 +25,7 @@ namespace autoware::behavior_path_planner { -using Point2d = autoware_universe_utils::Point2d; +using Point2d = autoware::universe_utils::Point2d; using tier4_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const 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 3082bcf26cba7..ee35790ee2ab1 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 @@ -57,7 +57,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) constexpr double straight_distance = 1.0; const Pose end_pose = use_back_ ? goal_pose - : autoware_universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); + : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); if (!planner_->makePlan(current_pose, end_pose)) { return {}; } @@ -85,7 +85,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - autoware_universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); if (distance < th_goal_distance) { last_path.points.erase(it, last_path.points.end()); break; @@ -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 (!autoware_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 a329e984fc7c0..2ac0b9493e67a 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 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 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 = autoware_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(autoware_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(autoware_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(autoware_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(autoware_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,9 +1266,9 @@ 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 = - autoware_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 = autoware_motion_utils::findNearestSegmentIndex( + 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, @@ -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 = autoware_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 = autoware_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 = autoware_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 = - autoware_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"); } @@ -1764,7 +1764,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath() const auto current_path_end = thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; - return autoware_universe_utils::calcDistance2d(current_path_end, self_pose) < + return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; } @@ -1789,9 +1789,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; const auto shift_start_idx = - autoware_motion_utils::findNearestIndex(path.points, start_pose.position); + autoware::motion_utils::findNearestIndex(path.points, start_pose.position); const auto shift_end_idx = - autoware_motion_utils::findNearestIndex(path.points, end_pose.position); + autoware::motion_utils::findNearestIndex(path.points, end_pose.position); const auto is_ignore_signal = [this](const lanelet::Id & id) { if (!ignore_signal_.has_value()) { @@ -1835,7 +1835,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() 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(autoware_motion_utils::calcSignedArcLength( + 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; }); @@ -1880,7 +1880,7 @@ bool GoalPlannerModule::checkObjectsCollision( std::vector obj_polygons; for (const auto & object : target_objects.objects) { - obj_polygons.push_back(autoware_universe_utils::toPolygon2d(object)); + obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); } /* Expand ego collision check polygon @@ -1889,7 +1889,7 @@ bool GoalPlannerModule::checkObjectsCollision( * - `extra_lateral_margin` adds the lateral margin on curves. */ std::vector ego_polygons_expanded{}; - const auto curvatures = autoware_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); @@ -1904,7 +1904,7 @@ bool GoalPlannerModule::checkObjectsCollision( extra_stopping_margin, std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto ego_polygon = autoware_universe_utils::toFootprint( + const auto ego_polygon = autoware::universe_utils::toFootprint( p.point.pose, planner_data->parameters.base_link2front + collision_check_margin + extra_stopping_margin, planner_data->parameters.base_link2rear + collision_check_margin, @@ -2202,7 +2202,7 @@ static std::vector filterOb for (const auto & target_lane : target_lanes) { const auto lane_poly = target_lane.polygon2d().basicPolygon(); for (const auto & filtered_object : filtered_objects.objects) { - const auto object_bbox = autoware_universe_utils::toPolygon2d(filtered_object); + const auto object_bbox = autoware::universe_utils::toPolygon2d(filtered_object); if (boost::geometry::within(object_bbox, lane_poly)) { within_filtered_objects.push_back(filtered_object); } @@ -2285,7 +2285,7 @@ std::pair GoalPlannerModule::isSafePath( lanelet::utils::conversion::toGeomMsgPt(fist_road_lane.centerline().front()); const double lane_yaw = lanelet::utils::getLaneletAngle(fist_road_lane, first_road_point); first_road_pose.position = first_road_point; - first_road_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); + first_road_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); // if current ego pose is before pull over lanes segment, use first road lanelet center pose if ( calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) < @@ -2362,10 +2362,10 @@ 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; + using autoware::motion_utils::createStopVirtualWallMarker; + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; @@ -2380,7 +2380,7 @@ void GoalPlannerModule::setDebugData() for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - autoware_universe_utils::appendMarkerArray(added, &debug_marker_); + autoware::universe_utils::appendMarkerArray(added, &debug_marker_); }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas @@ -2428,10 +2428,10 @@ void GoalPlannerModule::setDebugData() createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } - auto marker = autoware_universe_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, - autoware_universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware_universe_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); const double ego_z = planner_data_->self_odometry->pose.pose.position.z; for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { @@ -2439,19 +2439,19 @@ void GoalPlannerModule::setDebugData() const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); marker.points.push_back( - autoware_universe_utils::createPoint(current_point.x(), current_point.y(), ego_z)); + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z)); marker.points.push_back( - autoware_universe_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z)); } } debug_marker_.markers.push_back(marker); if (parameters_->safety_check_params.enable_safety_check) { - autoware_universe_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( goal_planner_utils::createLaneletPolygonMarkerArray( debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, "expanded_pull_over_lane_between_ego", - autoware_universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), + autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), &debug_marker_); } @@ -2578,7 +2578,7 @@ void GoalPlannerModule::printParkingPositionError() const real_shoulder_to_map_shoulder + parameters_->margin_from_boundary - dy; RCLCPP_INFO( getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, - autoware_universe_utils::rad2deg( + autoware::universe_utils::rad2deg( tf2::getYaw(current_pose.orientation) - tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); @@ -2634,7 +2634,7 @@ void GoalPlannerModule::GoalPlannerData::update( const PlannerData & planner_data_, const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, const std::shared_ptr goal_searcher_, - const autoware_universe_utils::LinearRing2d & vehicle_footprint_) + const autoware::universe_utils::LinearRing2d & vehicle_footprint_) { parameters = parameters_; ego_predicted_path_params = ego_predicted_path_params_; 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 564d22f2df2eb..04ee5278912ac 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 @@ -33,7 +33,7 @@ namespace autoware::behavior_path_planner { using autoware::lane_departure_checker::LaneDepartureChecker; -using autoware_universe_utils::calcOffsetPose; +using autoware::universe_utils::calcOffsetPose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; @@ -136,7 +136,7 @@ GoalCandidates GoalSearcher::search( // todo(kosuke55): fix orientation for inverseTransformPoint temporarily Pose center_pose = p.point.pose; center_pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); + autoware::universe_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -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(autoware_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{}; @@ -167,7 +167,7 @@ GoalCandidates GoalSearcher::search( search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, autoware_universe_utils::pose2transform(search_pose)); + transformVector(vehicle_footprint_, autoware::universe_utils::pose2transform(search_pose)); if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { // break here to exclude goals located laterally in no_parking_areas @@ -245,8 +245,8 @@ void GoalSearcher::countObjectsToAvoid( for (const auto & object : objects.objects) { for (const auto & p : current_center_line_path.points) { const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, autoware_universe_utils::pose2transform(p.point.pose)); - const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); + transformVector(vehicle_footprint_, autoware::universe_utils::pose2transform(p.point.pose)); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, transformed_vehicle_footprint); if (distance > parameters_.object_recognition_collision_check_hard_margins.back()) { continue; @@ -431,9 +431,9 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( void GoalSearcher::createAreaPolygons( std::vector original_search_poses, const std::shared_ptr & planner_data) { - using autoware_universe_utils::MultiPolygon2d; - using autoware_universe_utils::Point2d; - using autoware_universe_utils::Polygon2d; + using autoware::universe_utils::MultiPolygon2d; + using autoware::universe_utils::Point2d; + using autoware::universe_utils::Polygon2d; const double vehicle_width = planner_data->parameters.vehicle_width; const double base_link2front = planner_data->parameters.base_link2front; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 098405f2378b5..901fe351fc68b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -428,7 +428,7 @@ void GoalPlannerModuleManager::updateModuleParams( // object_recognition_collision_check_hard_margins, maximum_deceleration, shift_sampling_num or // parking_policy, there seems to be a problem when we use a temp value to check these values. - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; 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 46b4c66a7c9fa..beb27c67833ea 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,7 +101,7 @@ 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 = autoware_motion_utils::findNearestSegmentIndex( + 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}; @@ -111,10 +111,10 @@ std::optional ShiftPullOver::cropPrevModulePath( // add projected shift end pose to clipped points PathPointWithLaneId projected_point = clipped_points.back(); - const double offset = autoware_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); + autoware::universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); auto clipped_prev_module_path = prev_module_path; clipped_prev_module_path.points = clipped_points; @@ -131,7 +131,7 @@ std::optional ShiftPullOver::generatePullOverPath( // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = - autoware_universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); + autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( @@ -162,7 +162,7 @@ std::optional ShiftPullOver::generatePullOverPath( const Pose & shift_end_pose_prev_module_path = processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - autoware_universe_utils::inverseTransformPoint( + autoware::universe_utils::inverseTransformPoint( shift_end_pose.position, shift_end_pose_prev_module_path) .y; @@ -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 = autoware_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 = autoware_motion_utils::removeOverlapPoints(shifted_path.path.points); - autoware_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] : - autoware_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 f99a240f291a7..99bef9c243508 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 @@ -38,11 +38,11 @@ namespace autoware::behavior_path_planner::goal_planner_utils { -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 autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -116,9 +116,9 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const double ego_length_to_front = wheel_base + front_overhang; const double ego_width_to_front = !left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang); - autoware_universe_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; - const auto front_edge_glob = autoware_universe_utils::transformPoint( - front_edge_local, autoware_universe_utils::pose2transform(ego_pose)); + autoware::universe_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; + const auto front_edge_glob = autoware::universe_utils::transformPoint( + front_edge_local, autoware::universe_utils::pose2transform(ego_pose)); geometry_msgs::msg::Pose ego_front_pose; ego_front_pose.position = createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z); @@ -180,8 +180,8 @@ PredictedObjects filterObjectsByLateralDistance( } MarkerArray createPullOverAreaMarkerArray( - const autoware_universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, - const std_msgs::msg::ColorRGBA & color, const double z) + const autoware::universe_utils::MultiPolygon2d area_polygons, + const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) { MarkerArray marker_array{}; for (size_t i = 0; i < area_polygons.size(); ++i) { @@ -205,7 +205,7 @@ MarkerArray createPosesMarkerArray( MarkerArray msg{}; int32_t i = 0; for (const auto & pose : poses) { - Marker marker = autoware_universe_utils::createDefaultMarker( + Marker marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, createMarkerScale(0.5, 0.25, 0.25), color); marker.pose = pose; @@ -305,11 +305,11 @@ double calcLateralDeviationBetweenPaths( { double lateral_deviation = 0.0; for (const auto & target_point : target_path.points) { - const size_t nearest_index = autoware_motion_utils::findNearestIndex( + 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( + std::abs(autoware::universe_utils::calcLateralDeviation( reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); } return lateral_deviation; @@ -326,7 +326,7 @@ bool isReferencePath( std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) { const size_t end_idx = - autoware_motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + 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()) { @@ -336,9 +336,9 @@ std::optional cropPath(const PathWithLaneId & path, const Pose & // add projected end pose to clipped points PathPointWithLaneId projected_point = clipped_points.back(); const double offset = - autoware_motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + 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); + autoware::universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); auto clipped_path = path; clipped_path.points = clipped_points; @@ -354,7 +354,7 @@ PathWithLaneId cropForwardPoints( double sum_length = 0; for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { const double seg_length = - autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length + seg_length) { const auto cropped_points = std::vector{points.begin() + target_seg_idx, points.begin() + i}; @@ -393,7 +393,7 @@ 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 = autoware_motion_utils::findNearestSegmentIndex( + const size_t target_path_terminal_idx = autoware::motion_utils::findNearestSegmentIndex( reference_path.points, target_terminal_pose.position); PathWithLaneId clipped_path = @@ -401,9 +401,9 @@ PathWithLaneId extendPath( // shift clipped path to previous module path terminal pose const double lateral_shift_from_reference_path = - autoware_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 = autoware::universe_utils::calcOffsetPose( p.point.pose, 0, lateral_shift_from_reference_path, 0); } @@ -411,15 +411,15 @@ PathWithLaneId extendPath( const auto start_point = std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { const bool is_forward = - autoware_universe_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose) + autoware::universe_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose) .x > 0.0; - const bool is_close = autoware_universe_utils::calcDistance2d( + const bool is_close = autoware::universe_utils::calcDistance2d( p.point.pose.position, target_terminal_pose.position) < 0.1; return is_forward && !is_close; }); std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); - extended_path.points = autoware_motion_utils::removeOverlapPoints(extended_path.points); + extended_path.points = autoware::motion_utils::removeOverlapPoints(extended_path.points); return extended_path; } @@ -429,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 = autoware_motion_utils::findNearestSegmentIndex( + 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( + 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); @@ -445,7 +445,7 @@ std::vector createPathFootPrints( for (const auto & point : path.points) { const auto & pose = point.point.pose; footprints.push_back( - autoware_universe_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width)); } return footprints; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 89f5f715549bd..229f46f89377f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -37,7 +37,7 @@ namespace autoware::behavior_path_planner { using autoware::route_handler::Direction; -using autoware_universe_utils::StopWatch; +using autoware::universe_utils::StopWatch; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 3162b99b441d1..4071c39568987 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -44,10 +44,10 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_universe_utils::Polygon2d; using data::lane_change::LanesPolygon; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; 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 435769231e08c..0a5b31f9e32d2 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 @@ -29,7 +29,7 @@ namespace autoware::behavior_path_planner { -using autoware_universe_utils::appendMarkerArray; +using autoware::universe_utils::appendMarkerArray; using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( @@ -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 = autoware_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 = autoware_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/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 1e066d297f878..61ff54e8f99f3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -36,7 +36,7 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) void LaneChangeModuleManager::initParams(rclcpp::Node * node) { - using autoware_universe_utils::getOrDeclareParameter; + using autoware::universe_utils::getOrDeclareParameter; LaneChangeParameters p{}; @@ -281,7 +281,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto p = parameters_; 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 f00c68c8651fe..505bb3ef93340 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 autoware_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,12 +182,12 @@ 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 = autoware_motion_utils::calcArcLength(path.points); + const double path_length = autoware::motion_utils::calcArcLength(path.points); const size_t current_nearest_seg_idx = - autoware_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 = autoware_motion_utils::calcLongitudinalOffsetPose( + 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 @@ -389,10 +389,10 @@ void NormalLaneChange::insertStopPoint( // calculate distance from path front to the stationary object polygon on the ego lane. const auto polygon = - autoware_universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + 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 = autoware_motion_utils::calcLateralOffset(path.points, p_fp); + const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); + 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) { @@ -442,7 +442,7 @@ void NormalLaneChange::insertStopPoint( // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( - autoware_universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), lanelet::utils::combineLaneletsShape(status_.target_lanes) .polygon2d() .basicPolygon())) { @@ -690,7 +690,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } - const auto nearest_idx = autoware_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 = autoware_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 = autoware_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 = @@ -915,7 +915,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( auto prepare_segment = prev_module_output_.path; const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); @@ -992,11 +992,11 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( } const auto is_ahead_of_ego = [&path, ¤t_pose](const auto & object) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(object).outer(); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); double max_dist_ego_to_obj = std::numeric_limits::lowest(); for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware_universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); } @@ -1084,11 +1084,11 @@ void NormalLaneChange::filterAheadTerminalObjects( // ignore object that are ahead of terminal lane change start utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(object).outer(); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); // ignore object that are ahead of terminal lane change start auto distance_to_terminal_from_object = std::numeric_limits::max(); for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware_universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); Pose polygon_pose; polygon_pose.position = obj_p; polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; @@ -1668,7 +1668,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); } - const auto lane_changing_start_pose = autoware_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)); @@ -1753,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 = autoware_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); @@ -1895,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 autoware_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 = autoware_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) { @@ -1982,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 = autoware_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 = autoware_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); } @@ -2080,7 +2080,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = autoware_universe_utils::toPolygon2d(obj_pose, obj.shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 78db0ba0a1b4f..9a553cf97af5c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -34,8 +34,8 @@ namespace marker_utils::lane_change_markers { -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerScale; using geometry_msgs::msg::Point; MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) @@ -172,7 +172,7 @@ MarkerArray createDebugMarkerArray( MarkerArray debug_marker; const auto add = [&debug_marker](const MarkerArray & added) { - autoware_universe_utils::appendMarkerArray(added, &debug_marker); + autoware::universe_utils::appendMarkerArray(added, &debug_marker); }; if (!debug_data.execution_area.points.empty()) { 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 7edd2d063348b..d989a1e6d301d 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 @@ -58,11 +58,11 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_universe_utils::LineString2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -368,7 +368,7 @@ std::optional constructCandidatePath( LaneChangePath candidate_path; candidate_path.info = lane_change_info; - const auto lane_change_end_idx = autoware_motion_utils::findNearestIndex( + 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) { @@ -385,7 +385,7 @@ std::optional constructCandidatePath( continue; } const auto nearest_idx = - autoware_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; } @@ -404,13 +404,13 @@ std::optional constructCandidatePath( std::prev(prepare_segment.points.end() - 1)->point.pose; const auto & lane_change_start_from_shifted = std::next(shifted_path.path.points.begin())->point.pose; - const auto yaw_diff2 = std::abs(autoware_universe_utils::normalizeRadian( + const auto yaw_diff2 = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(prepare_segment_second_last_point.orientation) - tf2::getYaw(lane_change_start_from_shifted.orientation))); - if (yaw_diff2 > autoware_universe_utils::deg2rad(5.0)) { + if (yaw_diff2 > autoware::universe_utils::deg2rad(5.0)) { RCLCPP_DEBUG( get_logger(), "Excessive yaw difference %.3f which exceeds the 5 degrees threshold.", - autoware_universe_utils::rad2deg(yaw_diff2)); + autoware::universe_utils::rad2deg(yaw_diff2)); return std::nullopt; } } @@ -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 = autoware_motion_utils::findNearestIndex( + shift_line.start_idx = autoware::motion_utils::findNearestIndex( reference_path.points, lane_changing_start_pose.position); - shift_line.end_idx = - autoware_motion_utils::findNearestIndex(reference_path.points, lane_changing_end_pose.position); + shift_line.end_idx = 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 = autoware_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 = autoware_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; @@ -807,7 +807,7 @@ std::vector convertToPredictedPath( lane_change_parameters.minimum_lane_changing_velocity; const auto nearest_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); @@ -821,8 +821,8 @@ std::vector convertToPredictedPath( const double velocity = 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 = - autoware_motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + const auto pose = autoware::motion_utils::calcInterpolatedPose( + path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -836,8 +836,8 @@ std::vector convertToPredictedPath( const double velocity = lane_changing_velocity + lane_changing_acc * delta_t; const double length = lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset; - const auto pose = - autoware_motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + const auto pose = autoware::motion_utils::calcInterpolatedPose( + path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -870,7 +870,7 @@ bool isParkedObject( const auto & object_pose = object.initial_pose.pose; const auto object_closest_index = - autoware_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; @@ -879,7 +879,7 @@ bool isParkedObject( } const double lat_dist = - autoware_motion_utils::calcLateralOffset(path.points, object_pose.position); + autoware::motion_utils::calcLateralOffset(path.points, object_pose.position); lanelet::BasicLineString2d bound; double center_to_bound_buffer = 0.0; if (lat_dist > 0.0) { @@ -937,7 +937,7 @@ bool isParkedObject( const auto & obj_pose = object.initial_pose.pose; const auto & obj_shape = object.shape; - const auto obj_poly = autoware_universe_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; double max_dist_to_bound = std::numeric_limits::lowest(); @@ -996,7 +996,7 @@ bool passParkedObject( const auto & leading_obj = objects.at(*leading_obj_idx); auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = - autoware_universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); + autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return false; } @@ -1004,13 +1004,13 @@ bool passParkedObject( const auto & current_path_end = current_lane_path.points.back().point.pose.position; double min_dist_to_end_of_current_lane = std::numeric_limits::max(); 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 = - autoware_motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); + const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); + const double dist = 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 = autoware_motion_utils::calcSignedArcLength( + 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); } @@ -1061,14 +1061,14 @@ std::optional getLeadingStaticObjectIdx( continue; } - const double dist_back_to_obj = autoware_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 = autoware_motion_utils::calcSignedArcLength( + 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 @@ -1124,7 +1124,7 @@ ExtendedPredictedObject transform( } const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(*obj_pose, object.shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( t, *obj_pose, obj_vel_norm, obj_polygon); } @@ -1167,7 +1167,7 @@ Polygon2d getEgoCurrentFootprint( const auto base_to_rear = ego_info.rear_overhang_m; const auto width = ego_info.vehicle_width_m; - return autoware_universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); + return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); } bool isWithinIntersection( @@ -1255,13 +1255,13 @@ geometry_msgs::msg::Polygon createExecutionArea( const double lat_offset = width / 2.0 + additional_lat_offset; const auto p1 = - autoware_universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - autoware_universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - autoware_universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - autoware_universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); geometry_msgs::msg::Polygon polygon; polygon.points.push_back(create_point32(p1)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index a31172ac0050b..68547a491324d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -24,16 +24,16 @@ constexpr double epsilon = 1e-6; TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) { geometry_msgs::msg::Pose ego_pose; - const auto ego_yaw = autoware_universe_utils::deg2rad(0.0); - ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(ego_yaw); - ego_pose.position = autoware_universe_utils::createPoint(0, 0, 0); + const auto ego_yaw = autoware::universe_utils::deg2rad(0.0); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.position = autoware::universe_utils::createPoint(0, 0, 0); geometry_msgs::msg::Pose obj_pose; - const auto obj_yaw = autoware_universe_utils::deg2rad(0.0); - obj_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(obj_yaw); - obj_pose.position = autoware_universe_utils::createPoint(-4, 3, 0); + const auto obj_yaw = autoware::universe_utils::deg2rad(0.0); + obj_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(obj_yaw); + obj_pose.position = autoware::universe_utils::createPoint(-4, 3, 0); - const auto result = autoware_universe_utils::inverseTransformPose(obj_pose, ego_pose); + const auto result = autoware::universe_utils::inverseTransformPose(obj_pose, ego_pose); EXPECT_NEAR(result.position.x, -4, epsilon); EXPECT_NEAR(result.position.y, 3, epsilon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index 1420926f32bd7..3c62cc241b323 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -88,30 +88,30 @@ class BehaviorPathPlannerNode : public rclcpp::Node private: // subscriber - autoware_universe_utils::InterProcessPollingSubscriber route_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - autoware_universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - autoware_universe_utils::InterProcessPollingSubscriber velocity_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber velocity_subscriber_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber acceleration_subscriber_{this, "~/input/accel"}; - autoware_universe_utils::InterProcessPollingSubscriber scenario_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber scenario_subscriber_{ this, "~/input/scenario"}; - autoware_universe_utils::InterProcessPollingSubscriber perception_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber perception_subscriber_{ this, "~/input/perception"}; - autoware_universe_utils::InterProcessPollingSubscriber occupancy_grid_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber occupancy_grid_subscriber_{ this, "~/input/occupancy_grid_map"}; - autoware_universe_utils::InterProcessPollingSubscriber costmap_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber costmap_subscriber_{ this, "~/input/costmap"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber traffic_signals_subscriber_{this, "~/input/traffic_signals"}; - autoware_universe_utils::InterProcessPollingSubscriber lateral_offset_subscriber_{ + autoware::universe_utils::InterProcessPollingSubscriber lateral_offset_subscriber_{ this, "~/input/lateral_offset"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber operation_mode_subscriber_{ this, "/system/operation_mode/state", rclcpp::QoS{1}.transient_local()}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber external_limit_max_velocity_subscriber_{this, "/planning/scenario_planning/max_velocity"}; // publisher @@ -236,9 +236,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index ecd9dd37c363a..a76284b70b7d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -39,12 +39,12 @@ namespace autoware::behavior_path_planner { -using autoware_universe_utils::StopWatch; +using autoware::universe_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; -using DebugPublisher = autoware_universe_utils::DebugPublisher; +using DebugPublisher = autoware::universe_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; 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 378d8f9c5a5f2..0e586211bc9ee 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 @@ -114,9 +114,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -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 = autoware_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 + @@ -472,7 +472,7 @@ void BehaviorPathPlannerNode::run() if ( output.modified_goal && /* has changed modified goal */ ( - !planner_data_->prev_modified_goal || autoware_universe_utils::calcDistance2d( + !planner_data_->prev_modified_goal || autoware::universe_utils::calcDistance2d( planner_data_->prev_modified_goal->pose.position, output.modified_goal->pose.position) > 0.01)) { PoseWithUuidStamped modified_goal = *(output.modified_goal); @@ -566,29 +566,29 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb constexpr double scale_x = 1.0; constexpr double scale_y = 1.0; constexpr double scale_z = 1.0; - const auto scale = autoware_universe_utils::createMarkerScale(scale_x, scale_y, scale_z); + const auto scale = autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z); const auto desired_section_color = - autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); const auto required_section_color = - autoware_universe_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); + autoware::universe_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); // intersection turn signal info { const auto & turn_signal_info = debug_data.intersection_turn_signal_info; - auto desired_start_marker = autoware_universe_utils::createDefaultMarker( + auto desired_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_desired_start", 0L, Marker::SPHERE, scale, desired_section_color); - auto desired_end_marker = autoware_universe_utils::createDefaultMarker( + auto desired_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_desired_end", 0L, Marker::SPHERE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = autoware_universe_utils::createDefaultMarker( + auto required_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_required_start", 0L, Marker::SPHERE, scale, required_section_color); - auto required_end_marker = autoware_universe_utils::createDefaultMarker( + auto required_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_required_end", 0L, Marker::SPHERE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -604,19 +604,19 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb { const auto & turn_signal_info = debug_data.behavior_turn_signal_info; - auto desired_start_marker = autoware_universe_utils::createDefaultMarker( + auto desired_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_desired_start", 0L, Marker::CUBE, scale, desired_section_color); - auto desired_end_marker = autoware_universe_utils::createDefaultMarker( + auto desired_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_desired_end", 0L, Marker::CUBE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = autoware_universe_utils::createDefaultMarker( + auto required_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_required_start", 0L, Marker::CUBE, scale, required_section_color); - auto required_end_marker = autoware_universe_utils::createDefaultMarker( + auto required_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_required_end", 0L, Marker::CUBE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -642,18 +642,18 @@ void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) constexpr double color_a = 0.999; const auto current_time = path.header.stamp; - auto left_marker = autoware_universe_utils::createDefaultMarker( + auto left_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "left_bound", 0L, Marker::LINE_STRIP, - autoware_universe_utils::createMarkerScale(scale_x, scale_y, scale_z), - autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto lb : path.left_bound) { left_marker.points.push_back(lb); } - auto right_marker = autoware_universe_utils::createDefaultMarker( + auto right_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "right_bound", 0L, Marker::LINE_STRIP, - autoware_universe_utils::createMarkerScale(scale_x, scale_y, scale_z), - autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto rb : path.right_bound) { right_marker.points.push_back(rb); } @@ -744,7 +744,7 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = autoware_motion_utils::convertToPath( + 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(); @@ -830,7 +830,7 @@ void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPt SetParametersResult BehaviorPathPlannerNode::onSetParam( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; rcl_interfaces::msg::SetParametersResult result; 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 71714cda9052d..e66eec2a1e0a4 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 = autoware_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)) { @@ -466,10 +466,10 @@ void PlannerManager::publishDebugRootReferencePath( { using visualization_msgs::msg::Marker; MarkerArray array; - Marker m = autoware_universe_utils::createDefaultMarker( + Marker m = autoware::universe_utils::createDefaultMarker( "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP, - autoware_universe_utils::createMarkerScale(1.0, 1.0, 1.0), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); + autoware::universe_utils::createMarkerScale(1.0, 1.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position); array.markers.push_back(m); m.points.clear(); 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 f79087d7df405..da8657c82c58e 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 @@ -35,7 +35,7 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnStraightLin Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(10.7f, -1.7f, 0.0); const size_t vehicle_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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); @@ -51,7 +51,7 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnDiagonalLin Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(0.1f, 0.1f, 0.0); const size_t vehicle_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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 ec9a67a40e052..0ec7841f77cfe 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 @@ -102,7 +102,7 @@ struct DrivableAreaInfo struct Obstacle { geometry_msgs::msg::Pose pose; - autoware_universe_utils::Polygon2d poly; + autoware::universe_utils::Polygon2d poly; bool is_left{true}; }; std::vector drivable_lanes{}; @@ -254,7 +254,7 @@ struct PlannerData template size_t findEgoIndex(const std::vector & points) const { - return autoware_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 autoware_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 898414c29eaa4..d11c3181526d2 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 @@ -59,10 +59,10 @@ namespace autoware::behavior_path_planner using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::generateUUID; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::generateUUID; using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; @@ -146,7 +146,7 @@ class SceneModuleInterface updateData(); const auto output = isWaitingApproval() ? planWaitingApproval() : plan(); try { - autoware_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,7 +566,7 @@ class SceneModuleInterface StopFactor stop_factor; stop_factor.stop_pose = stop_pose_.value(); - stop_factor.dist_to_stop_pose = autoware_motion_utils::calcSignedArcLength( + 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 02e29e028f74c..5b9c0389e29c5 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 autoware::motion_utils::createDeadLineVirtualWallMarker; +using autoware::motion_utils::createSlowDownVirtualWallMarker; +using autoware::motion_utils::createStopVirtualWallMarker; +using autoware::universe_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -111,7 +111,7 @@ class SceneModuleManagerInterface void publishVirtualWall() const { - using autoware_universe_utils::appendMarkerArray; + using autoware::universe_utils::appendMarkerArray; MarkerArray markers{}; @@ -155,7 +155,7 @@ class SceneModuleManagerInterface void publishMarker() const { - using autoware_universe_utils::appendMarkerArray; + using autoware::universe_utils::appendMarkerArray; MarkerArray info_markers{}; MarkerArray debug_markers{}; @@ -271,7 +271,7 @@ class SceneModuleManagerInterface protected: void initInterface(rclcpp::Node * node, const std::vector & rtc_types) { - using autoware_universe_utils::getOrDeclareParameter; + using autoware::universe_utils::getOrDeclareParameter; // init manager configuration { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp index 4151f4dcacf19..f2318e9d5c1ad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp @@ -26,62 +26,62 @@ using std_msgs::msg::ColorRGBA; inline ColorRGBA red(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(1., 0., 0., a); + return autoware::universe_utils::createMarkerColor(1., 0., 0., a); } inline ColorRGBA green(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(0., 1., 0., a); + return autoware::universe_utils::createMarkerColor(0., 1., 0., a); } inline ColorRGBA blue(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(0., 0., 1., a); + return autoware::universe_utils::createMarkerColor(0., 0., 1., a); } inline ColorRGBA yellow(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(1., 1., 0., a); + return autoware::universe_utils::createMarkerColor(1., 1., 0., a); } inline ColorRGBA aqua(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(0., 1., 1., a); + return autoware::universe_utils::createMarkerColor(0., 1., 1., a); } inline ColorRGBA magenta(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(1., 0., 1., a); + return autoware::universe_utils::createMarkerColor(1., 0., 1., a); } inline ColorRGBA medium_orchid(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(0.729, 0.333, 0.827, a); + return autoware::universe_utils::createMarkerColor(0.729, 0.333, 0.827, a); } inline ColorRGBA light_pink(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(1., 0.713, 0.756, a); + return autoware::universe_utils::createMarkerColor(1., 0.713, 0.756, a); } inline ColorRGBA light_yellow(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(1., 1., 0.878, a); + return autoware::universe_utils::createMarkerColor(1., 1., 0.878, a); } inline ColorRGBA light_steel_blue(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(0.690, 0.768, 0.870, a); + return autoware::universe_utils::createMarkerColor(0.690, 0.768, 0.870, a); } inline ColorRGBA white(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(1., 1., 1., a); + return autoware::universe_utils::createMarkerColor(1., 1., 1., a); } inline ColorRGBA grey(float a = 0.99) { - return autoware_universe_utils::createMarkerColor(.5, .5, .5, a); + return autoware::universe_utils::createMarkerColor(.5, .5, .5, a); } inline std::vector colors_list(float a = 0.99) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp index fc0efc38ae617..819800281fcc5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp @@ -40,9 +40,9 @@ using autoware::behavior_path_planner::utils::path_safety_checker::CollisionChec using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index fa3236d7fd868..94ed8af3c82b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp @@ -239,8 +239,8 @@ class TurnSignalDecider const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) const { - using autoware_universe_utils::pose2transform; - using autoware_universe_utils::transformVector; + using autoware::universe_utils::pose2transform; + using autoware::universe_utils::transformVector; using boost::geometry::intersects; const auto footprint = vehicle_info.createFootprint(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index de29886c27b4e..afe88ac04f302 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -36,13 +36,13 @@ using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; -using autoware_universe_utils::LineString2d; -using autoware_universe_utils::MultiLineString2d; -using autoware_universe_utils::MultiPoint2d; -using autoware_universe_utils::MultiPolygon2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; -using autoware_universe_utils::Segment2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::MultiLineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Segment2d; using SegmentRtree = boost::geometry::index::rtree>; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 8dfc590230064..4fd6fce325f49 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -70,7 +70,7 @@ bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::Cons bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); bool isPolygonOverlapLanelet( - const PredictedObject & object, const autoware_universe_utils::Polygon2d & lanelet_polygon); + const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon); bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 421c495555c3c..313f9df471938 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -31,8 +31,8 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 92b7bab5c26ba..3f1094dfd70b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -32,13 +32,13 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::universe_utils::calcYawDeviation; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; -using autoware_universe_utils::calcYawDeviation; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 1e31bafe3fa0b..8164ef659b7b1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -29,7 +29,7 @@ #include namespace autoware::behavior_path_planner { -using autoware_universe_utils::generateUUID; +using autoware::universe_utils::generateUUID; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathPointWithLaneId; 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 9196123c4e73a..5c1eee92c2a5d 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 @@ -47,8 +47,8 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using autoware::route_handler::RouteHandler; -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; @@ -93,11 +93,11 @@ FrenetPoint convertToFrenetPoint( FrenetPoint frenet_point; const double longitudinal_length = - autoware_motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, search_point_geom); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, search_point_geom); frenet_point.length = - autoware_motion_utils::calcSignedArcLength(points, 0, seg_idx) + longitudinal_length; + autoware::motion_utils::calcSignedArcLength(points, 0, seg_idx) + longitudinal_length; frenet_point.distance = - autoware_motion_utils::calcLateralOffset(points, search_point_geom, seg_idx); + autoware::motion_utils::calcLateralOffset(points, search_point_geom, seg_idx); return frenet_point; } @@ -147,7 +147,7 @@ bool checkCollisionWithExtraStoppingMargin( * @return Has collision or not */ bool checkCollisionBetweenPathFootprintsAndObjects( - const autoware_universe_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, + const autoware::universe_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin); /** @@ -155,7 +155,7 @@ bool checkCollisionBetweenPathFootprintsAndObjects( * @return Has collision or not */ bool checkCollisionBetweenFootprintAndObjects( - const autoware_universe_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, + const autoware::universe_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin); /** @@ -344,12 +344,12 @@ size_t findNearestSegmentIndex( const double yaw_threshold) { const auto nearest_idx = - autoware_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 autoware_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/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 154ceb6dc6dd5..8c9b4ef35d5af 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -31,12 +31,12 @@ namespace marker_utils { using autoware::behavior_path_planner::utils::calcPathArcLengthArray; -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; -using autoware_universe_utils::createPoint; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; @@ -49,13 +49,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -214,7 +214,7 @@ MarkerArray createShiftLengthMarkerArray( Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.9)); - marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); marker.points.reserve(shift_distance.size()); for (size_t i = 0; i < shift_distance.size(); ++i) { @@ -271,7 +271,7 @@ MarkerArray createLaneletsAreaMarkerArray( "map", current_time, ns, static_cast(lanelet.id()), Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); - marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); if (!lanelet.polygon3d().empty()) { marker.points.reserve(lanelet.polygon3d().size() + 1); @@ -299,7 +299,7 @@ MarkerArray createPolygonMarkerArray( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast(lane_id), Marker::LINE_STRIP, createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); - marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); if (!polygon.points.empty()) { marker.points.reserve(polygon.points.size() + 1); 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 5a785a71b863d..73f81ce59844c 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 @@ -33,16 +33,16 @@ namespace autoware::behavior_path_planner { -using autoware_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 = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return autoware_motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); } @@ -83,8 +83,9 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( } // Closest ego segment - const size_t ego_seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - extended_path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + 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 const auto intersection_turn_signal_info = getIntersectionTurnSignalInfo( @@ -231,20 +232,20 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } const size_t front_nearest_seg_idx = - autoware_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 = - autoware_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 = autoware_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 = autoware_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); @@ -280,9 +281,9 @@ 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 = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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( + 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); @@ -407,9 +408,9 @@ TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( const auto get_distance = [&](const Pose & input_point) { const size_t nearest_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return autoware_motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx) - base_link2front_; @@ -434,9 +435,9 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( { const auto get_distance = [&](const Pose & input_point) { const size_t nearest_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return autoware_motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); }; @@ -557,9 +558,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]); } - autoware_motion_utils::insertOrientation(converted_centerline, true); + autoware::motion_utils::insertOrientation(converted_centerline, true); - const double length = autoware_motion_utils::calcArcLength(converted_centerline); + const double length = autoware::motion_utils::calcArcLength(converted_centerline); // Create resampling intervals const double resampling_interval = 1.0; @@ -569,20 +570,21 @@ geometry_msgs::msg::Pose TurnSignalDecider::get_required_end_point( } // Insert terminal point - if (length - resampling_arclength.back() < autoware_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 = - autoware_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) { const double yaw = tf2::getYaw(resampled_centerline.at(i).orientation); - const double yaw_diff = autoware_universe_utils::normalizeRadian(yaw - terminal_yaw); - if (std::fabs(yaw_diff) < autoware_universe_utils::deg2rad(intersection_angle_threshold_deg_)) { + const double yaw_diff = autoware::universe_utils::normalizeRadian(yaw - terminal_yaw); + if ( + std::fabs(yaw_diff) < autoware::universe_utils::deg2rad(intersection_angle_threshold_deg_)) { return resampled_centerline.at(i); } } @@ -597,9 +599,9 @@ void TurnSignalDecider::set_intersection_info( { const auto get_distance = [&](const Pose & input_point) { const size_t nearest_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return autoware_motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); }; @@ -639,9 +641,9 @@ void TurnSignalDecider::initialize_intersection_info() geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation( const Point & src_point, const Point & dst_point) { - const double pitch = autoware_universe_utils::calcElevationAngle(src_point, dst_point); - const double yaw = autoware_universe_utils::calcAzimuthAngle(src_point, dst_point); - return autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); + const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); + return autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( @@ -652,7 +654,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { - using autoware_universe_utils::getPose; + using autoware::universe_utils::getPose; const auto & p = parameters; const auto & rh = route_handler; 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 e3ebc07a8ce39..6e028cec2ed1b 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 @@ -50,8 +50,8 @@ void reuse_previous_poses( std::vector cropped_curvatures; 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( + 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 LineString2d left_bound; @@ -65,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 = - autoware_motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); - const auto deviation = - autoware_motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + autoware::motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + const auto deviation = 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)); @@ -87,27 +87,29 @@ void reuse_previous_poses( } if (cropped_poses.empty()) { const auto resampled_path_points = - autoware_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 - : autoware_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 = 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( + 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(autoware_motion_utils::calcInterpolatedPose(path.points, arc_length)); + cropped_poses.push_back( + autoware::motion_utils::calcInterpolatedPose(path.points, arc_length)); } - prev_poses = autoware_motion_utils::removeOverlapPoints(cropped_poses); + prev_poses = autoware::motion_utils::removeOverlapPoints(cropped_poses); prev_curvatures = cropped_curvatures; } @@ -162,7 +164,7 @@ void apply_arc_length_range_smoothing( auto arc_length = boost::geometry::distance( bound_projections[path_idx].point, convert_point(bound[bound_idx + 1])); const auto update_arc_length_and_bound_expansions = [&](auto idx) { - arc_length += autoware_universe_utils::calcDistance2d(bound[idx - 1], bound[idx]); + arc_length += autoware::universe_utils::calcDistance2d(bound[idx - 1], bound[idx]); bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]); }; for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { @@ -201,7 +203,7 @@ void apply_bound_change_rate_limit( if (distances.empty()) return; const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { if (exp[from] > exp[to]) { - const auto arc_length = autoware_universe_utils::calcDistance2d(bound[from], bound[to]); + const auto arc_length = autoware::universe_utils::calcDistance2d(bound[from], bound[to]); const auto smoothed_dist = exp[from] - arc_length * max_rate; exp[to] = std::max(exp[to], smoothed_dist); } @@ -285,12 +287,12 @@ void expand_bound( for (auto idx = 1LU; idx < bound.size(); ++idx) { bool is_intersecting = false; for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { - const auto intersection = autoware_universe_utils::intersect( + const auto intersection = autoware::universe_utils::intersect( bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); if ( intersection && - autoware_universe_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && - autoware_universe_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + autoware::universe_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + autoware::universe_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { idx = succ_idx; is_intersecting = true; } @@ -303,7 +305,7 @@ void expand_bound( std::vector calculate_smoothed_curvatures( const std::vector & poses, const size_t smoothing_window_size) { - const auto curvatures = autoware_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; @@ -364,7 +366,7 @@ void expand_drivable_area( { // skip if no bounds or not enough points to calculate path curvature if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); const auto & params = planner_data->drivable_area_expansion_parameters; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index b01669871276a..a25a33c1668fa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -37,7 +37,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 { const auto angle = tf2::getYaw(pose.orientation); return translate_polygon( - autoware_universe_utils::rotatePolygon(base_footprint, angle), pose.position.x, + autoware::universe_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } 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 87e7d297c65c7..a2d701c3d3871 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 @@ -55,8 +55,8 @@ std::vector removeSharpPoints(const std::vector & points) const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - const auto dist_1to2 = autoware_universe_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = autoware_universe_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware::universe_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = autoware::universe_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; @@ -82,15 +82,15 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { - using autoware_universe_utils::calcAzimuthAngle; - using autoware_universe_utils::calcDistance2d; - using autoware_universe_utils::normalizeRadian; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::normalizeRadian; std::optional closest_idx{std::nullopt}; 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 = - autoware_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(autoware_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 autoware_motion_utils::findNearestSegmentIndex(points, target_point); + return autoware::motion_utils::findNearestSegmentIndex(points, target_point); } template @@ -119,9 +119,9 @@ size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Pose & target_point, const double yaw_threshold) { - using autoware_universe_utils::calcAzimuthAngle; - using autoware_universe_utils::calcDistance2d; - using autoware_universe_utils::normalizeRadian; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::normalizeRadian; std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); @@ -132,7 +132,7 @@ size_t findNearestSegmentIndexFromLateralDistance( if (yaw_threshold < std::abs(yaw)) { continue; } - const double lon_dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( + 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 = [&]() { @@ -143,7 +143,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return calcDistance2d(points.at(seg_idx + 1), target_point.position); } return std::abs( - autoware_motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); + autoware::motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; @@ -155,7 +155,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return *closest_idx; } - return autoware_motion_utils::findNearestSegmentIndex(points, target_point.position); + return autoware::motion_utils::findNearestSegmentIndex(points, target_point.position); } bool checkHasSameLane( @@ -177,9 +177,9 @@ 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 = autoware_motion_utils::calcLongitudinalOffsetToSegment( + const double offset_length = autoware::motion_utils::calcLongitudinalOffsetToSegment( points, nearest_segment_idx, pose.position); - const auto offset_point = autoware_motion_utils::calcLongitudinalOffsetPoint( + 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); @@ -189,9 +189,9 @@ 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 = autoware_motion_utils::calcLongitudinalOffsetToSegment( + const double offset_length = autoware::motion_utils::calcLongitudinalOffsetToSegment( points, nearest_segment_idx, pose.position); - const auto offset_point = autoware_motion_utils::calcLongitudinalOffsetPoint( + 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); @@ -246,7 +246,7 @@ std::optional> intersectBound( static_cast(bound.size()) - 1, static_cast(std::max(seg_idx1, seg_idx2)) + 1 + 5)); for (int i = start_idx; i < static_cast(end_idx); ++i) { const auto intersect_point = - autoware_universe_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); + autoware::universe_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); if (intersect_point) { std::pair result; result.first = static_cast(i); @@ -262,7 +262,7 @@ double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { - using autoware_universe_utils::calcSquaredDistance2d; + using autoware::universe_utils::calcSquaredDistance2d; const auto & a = segment_start_point; const auto & b = segment_end_point; @@ -294,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 = autoware_motion_utils::calcLongitudinalOffsetToSegment( + const double lon_dist_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( bound_points, min_dist_seg_idx, target_point); const double lat_dist_to_segment = - autoware_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}; } @@ -343,7 +343,7 @@ std::vector generatePolygonInsideBounds( if (!intersection) { continue; } - const double lon_dist = autoware_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}; @@ -438,7 +438,7 @@ std::vector concatenateTwoPolygons( double min_dist_to_intersection = std::numeric_limits::max(); PolygonPoint closest_intersect_point; for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { - const auto intersection = autoware_universe_utils::intersect( + const auto intersection = autoware::universe_utils::intersect( get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, get_in_poly().at(i).point, get_in_poly().at(i + 1).point); if (!intersection) { @@ -455,7 +455,7 @@ std::vector concatenateTwoPolygons( const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0}; const double dist_to_intersection = - autoware_universe_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); + autoware::universe_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); if (dist_to_intersection < min_dist_to_intersection) { closest_idx = i; min_dist_to_intersection = dist_to_intersection; @@ -569,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 = autoware_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(); @@ -578,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 = autoware_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(); @@ -606,7 +606,7 @@ std::vector updateBoundary( const auto & start_poly = polygon.front(); const auto & end_poly = polygon.back(); - const double front_offset = autoware_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 = @@ -641,7 +641,7 @@ std::vector updateBoundary( namespace autoware::behavior_path_planner::utils { -using autoware_universe_utils::Point2d; +using autoware::universe_utils::Point2d; std::optional getOverlappedLaneletId(const std::vector & lanes) { @@ -853,7 +853,7 @@ void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, const bool is_driving_forward) { - using autoware_universe_utils::calcOffsetPose; + using autoware::universe_utils::calcOffsetPose; // remove path points which is close to the previous point PathWithLaneId resampled_path{}; @@ -865,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 = - autoware_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)); } @@ -874,7 +874,7 @@ void generateDrivableArea( // add last point of path if enough far from the one of resampled path constexpr double th_last_point_distance = 0.3; if ( - autoware_universe_utils::calcDistance2d( + autoware::universe_utils::calcDistance2d( resampled_path.points.back().point.pose.position, path.points.back().point.pose.position) > th_last_point_distance) { resampled_path.points.push_back(path.points.back()); @@ -947,7 +947,7 @@ void generateDrivableArea( p_line.push_back(p2); bool intersection_found = false; for (size_t j = i + 2; j < bound.size() - 1; j++) { - const double distance = autoware_universe_utils::calcDistance2d(bound.at(i), bound.at(j)); + const double distance = autoware::universe_utils::calcDistance2d(bound.at(i), bound.at(j)); if (distance > intersection_check_distance) { break; } @@ -1041,12 +1041,12 @@ void extractObstaclesFromDrivableArea( const auto & obj_pos = obstacle.pose.position; // get edge points of the object - const size_t nearest_path_idx = - autoware_motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon + const size_t nearest_path_idx = 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 - edge_points.push_back(autoware_universe_utils::createPoint( + edge_points.push_back(autoware::universe_utils::createPoint( obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(), path.points.at(nearest_path_idx).point.pose.position.z)); } @@ -1303,9 +1303,9 @@ std::pair, bool> getBoundWithFreeSpaceAreas( const std::vector & other_side_bound, const std::shared_ptr planner_data, const bool is_left) { - using autoware_universe_utils::getPose; - using autoware_universe_utils::pose2transform; - using autoware_universe_utils::transformVector; + using autoware::universe_utils::getPose; + using autoware::universe_utils::pose2transform; + using autoware::universe_utils::transformVector; using lanelet::utils::to2D; using lanelet::utils::conversion::toGeomMsgPt; using lanelet::utils::conversion::toLaneletPoint; @@ -1383,12 +1383,12 @@ std::pair, bool> getBoundWithFreeSpaceAreas( return bound; } - const auto p_offset = autoware_universe_utils::calcOffsetPose( + const auto p_offset = autoware::universe_utils::calcOffsetPose( ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); std::vector ret; for (size_t i = 1; i < bound.size(); ++i) { - const auto intersect = autoware_universe_utils::intersect( + const auto intersect = autoware::universe_utils::intersect( ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), toGeomMsgPt(bound.at(i))); @@ -1493,7 +1493,7 @@ std::vector postProcess( const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); if (bound.empty()) { bound.push_back(cp); - } else if (autoware_universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + } else if (autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { bound.push_back(cp); } } @@ -1558,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 = autoware_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); @@ -1597,7 +1597,8 @@ std::vector postProcess( // Insert middle points for (size_t i = start_idx + 1; i <= goal_idx; ++i) { const auto & next_point = tmp_bound.at(i); - const double dist = autoware_universe_utils::calcDistance2d(processed_bound.back(), next_point); + const double dist = + autoware::universe_utils::calcDistance2d(processed_bound.back(), next_point); if (dist > overlap_threshold) { processed_bound.push_back(next_point); } @@ -1605,7 +1606,7 @@ std::vector postProcess( // Insert a goal point if ( - autoware_universe_utils::calcDistance2d(processed_bound.back(), goal_point) > + autoware::universe_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { processed_bound.push_back(goal_point); } @@ -1620,7 +1621,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 autoware_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/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 875de7ff18400..e4f1919b08dbc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -21,9 +21,9 @@ namespace autoware::behavior_path_planner { -using autoware_universe_utils::createQuaternionFromYaw; -using autoware_universe_utils::normalizeRadian; -using autoware_universe_utils::transformPose; +using autoware::universe_utils::createQuaternionFromYaw; +using autoware::universe_utils::normalizeRadian; +using autoware::universe_utils::transformPose; int discretizeAngle(const double theta, const int theta_size) { 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 92d955edb4542..113b87f4c0032 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 @@ -31,11 +31,11 @@ #include #include -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::inverseTransformPoint; -using autoware_universe_utils::normalizeRadian; -using autoware_universe_utils::transformPose; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::inverseTransformPoint; +using autoware::universe_utils::normalizeRadian; +using autoware::universe_utils::transformPose; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; @@ -70,7 +70,7 @@ PathWithLaneId GeometricParallelParking::getFullPath() const } PathWithLaneId filtered_path = path; - filtered_path.points = autoware_motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); return filtered_path; } @@ -132,10 +132,10 @@ std::vector GeometricParallelParking::generatePullOverPaths( // check the continuity of straight path and arc path const Pose & road_path_last_pose = straight_path.points.back().point.pose; const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; - const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(road_path_last_pose.orientation) - tf2::getYaw(arc_path_first_pose.orientation))); const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); - if (yaw_diff > autoware_universe_utils::deg2rad(5.0) || distance > 0.1) { + if (yaw_diff > autoware::universe_utils::deg2rad(5.0) || distance > 0.1) { return std::vector{}; } @@ -282,10 +282,10 @@ bool GeometricParallelParking::planPullOut( // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; - const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(road_path_first_pose.orientation) - tf2::getYaw(arc_path_last_pose.orientation))); const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose); - if (yaw_diff > autoware_universe_utils::deg2rad(5.0) || distance > 0.1) { + if (yaw_diff > autoware::universe_utils::deg2rad(5.0) || distance > 0.1) { continue; } @@ -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 = autoware_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) { @@ -399,7 +399,7 @@ std::vector GeometricParallelParking::planOneTrial( // combine road and shoulder lanes // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane lanelet::ConstLanelets lanes{}; - autoware_universe_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); + autoware::universe_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); for (const auto & lane : road_lanes) { if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) { lanes.push_back(lane); 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 3a9f70c16fb35..69b0deb8c4c5d 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 @@ -22,7 +22,7 @@ namespace autoware::behavior_path_planner::utils::parking_departure { -using autoware_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 = autoware_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 = autoware_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 ca72be6fa1568..469be03eb905c 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 @@ -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 = autoware_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); @@ -71,16 +71,16 @@ bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::Cons } bool isPolygonOverlapLanelet( - const PredictedObject & object, const autoware_universe_utils::Polygon2d & lanelet_polygon) + const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon) { - const auto object_polygon = autoware_universe_utils::toPolygon2d(object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon) { - const auto object_polygon = autoware_universe_utils::toPolygon2d(object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } @@ -273,8 +273,8 @@ std::vector createPredictedPath( length = current_velocity * t_with_delay + 0.5 * acceleration * t_with_delay * t_with_delay; } - const auto pose = - autoware_motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); + const auto pose = autoware::motion_utils::calcInterpolatedPose( + path_points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -315,7 +315,7 @@ ExtendedPredictedObject transform( for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(*obj_pose, object.shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths[i].path.emplace_back( t, *obj_pose, obj_velocity, obj_polygon); } 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 57ce48c540046..7bb8c13aa65fb 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 @@ -55,13 +55,13 @@ bool isTargetObjectFront( { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_offset_pose = - autoware_universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon 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 (autoware_universe_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + const auto obj_point = autoware::universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (autoware::universe_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { return true; } } @@ -75,13 +75,13 @@ bool isTargetObjectFront( { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_point = - autoware_universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon 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 (autoware_motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { + const auto obj_point = autoware::universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (autoware::motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; } } @@ -111,13 +111,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - autoware_universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - autoware_universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -125,16 +125,16 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return autoware_universe_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : autoware_universe_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(obj_pose, shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { return obj_polygon; } @@ -145,8 +145,8 @@ Polygon2d createExtendedPolygon( double min_y = std::numeric_limits::max(); const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & polygon_p : obj_polygon_outer) { - const auto obj_p = autoware_universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const auto transformed_p = autoware_universe_utils::inverseTransformPoint(obj_p, obj_pose); + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto transformed_p = autoware::universe_utils::inverseTransformPoint(obj_p, obj_pose); max_x = std::max(transformed_p.x, max_x); min_x = std::min(transformed_p.x, min_x); @@ -168,13 +168,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - autoware_universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); const auto p2 = - autoware_universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); const auto p3 = - autoware_universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); const auto p4 = - autoware_universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -182,9 +182,9 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return autoware_universe_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : autoware_universe_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } Polygon2d createExtendedPolygonAlongPath( @@ -208,7 +208,7 @@ Polygon2d createExtendedPolygonAlongPath( debug.lat_offset = lat_offset; } - const auto lon_offset_pose = autoware_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,57 +216,57 @@ Polygon2d createExtendedPolygonAlongPath( } const size_t start_idx = - autoware_motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); - const size_t end_idx = autoware_motion_utils::findNearestSegmentIndex( + 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; { - const auto p_offset = - autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + const auto p_offset = autoware::universe_utils::calcOffsetPose( + base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = start_idx + 1; i < end_idx + 1; ++i) { - const auto p = autoware_universe_utils::getPose(planned_path.points.at(i)); - const auto p_offset = autoware_universe_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); + const auto p = autoware::universe_utils::getPose(planned_path.points.at(i)); + const auto p_offset = autoware::universe_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = autoware_universe_utils::calcOffsetPose( + const auto p_offset = autoware::universe_utils::calcOffsetPose( lon_offset_pose.value(), base_to_front, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = autoware_universe_utils::calcOffsetPose( + const auto p_offset = autoware::universe_utils::calcOffsetPose( lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = end_idx; i > start_idx; --i) { - const auto p = autoware_universe_utils::getPose(planned_path.points.at(i)); - const auto p_offset = autoware_universe_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); + const auto p = autoware::universe_utils::getPose(planned_path.points.at(i)); + const auto p_offset = autoware::universe_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = autoware_universe_utils::calcOffsetPose( + const auto p_offset = autoware::universe_utils::calcOffsetPose( base_link_pose, backward_lon_offset, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = - autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + const auto p_offset = autoware::universe_utils::calcOffsetPose( + base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } - return autoware_universe_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : autoware_universe_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } std::vector createExtendedPolygonsFromPoseWithVelocityStamped( @@ -283,7 +283,7 @@ std::vector createExtendedPolygonsFromPoseWithVelocityStamped( const double width = vehicle_info.vehicle_width_m + lat_margin * 2; const auto polygon = - autoware_universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); + autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); polygons.push_back(polygon); } @@ -350,7 +350,7 @@ std::optional calcInterpolatedPoseWithVelocity( const double time_step = pt.time - prev_pt.time; const double ratio = std::clamp(offset / time_step, 0.0, 1.0); const auto interpolated_pose = - autoware_universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); + autoware::universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); const double interpolated_velocity = interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); return PoseWithVelocityStamped{relative_time, interpolated_pose, interpolated_velocity}; @@ -378,7 +378,7 @@ std::optional getInterpolatedPoseWithVelocity const auto & velocity = interpolation_result->velocity; const auto ego_polygon = - autoware_universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); + autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon}; } @@ -406,7 +406,7 @@ std::optional getInterpolatedPoseWithVelocity const auto & pose = interpolation_result->pose; const auto & velocity = interpolation_result->velocity; - const auto obj_polygon = autoware_universe_utils::toPolygon2d(pose, shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(pose, shape); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, obj_polygon}; } @@ -692,10 +692,10 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) CollisionCheckDebug debug; debug.current_obj_pose = obj.initial_pose.pose; debug.extended_obj_polygon = - autoware_universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + autoware::universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); debug.obj_shape = obj.shape; debug.current_twist = obj.initial_twist.twist; - return {autoware_universe_utils::toBoostUUID(obj.uuid), debug}; + return {autoware::universe_utils::toBoostUUID(obj.uuid), debug}; } void updateCollisionCheckDebugMap( 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 fa2c45b74942e..9572e5a92605f 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 @@ -51,10 +51,10 @@ std::string toStr(const std::vector & v) namespace autoware::behavior_path_planner { -using autoware_motion_utils::findNearestIndex; -using autoware_motion_utils::insertOrientation; -using autoware_motion_utils::removeFirstInvalidOrientationPoints; -using autoware_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 1e2ceac5cd19a..46ca155d04daa 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 @@ -49,8 +49,8 @@ std::vector calcPathArcLengthArray( out.push_back(sum); for (size_t i = bounded_start + 1; i < bounded_end; ++i) { - sum += - autoware_universe_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); + sum += autoware::universe_utils::calcDistance2d( + path.points.at(i).point, path.points.at(i - 1).point); out.push_back(sum); } return out; @@ -98,7 +98,7 @@ PathWithLaneId resamplePathWithSpline( // Get lane ids that are not duplicated std::vector s_in; std::unordered_set unique_lane_ids; - const auto s_vec = autoware_motion_utils::calcSignedArcLengthPartialSum( + 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); @@ -130,7 +130,7 @@ PathWithLaneId resamplePathWithSpline( // Insert Stop Point const auto closest_stop_dist = - autoware_motion_utils::calcDistanceToForwardStopPoint(transformed_path); + 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) { @@ -153,7 +153,7 @@ PathWithLaneId resamplePathWithSpline( std::sort(s_out.begin(), s_out.end()); - return autoware_motion_utils::resamplePath(path, s_out); + return autoware::motion_utils::resamplePath(path, s_out); } size_t getIdxByArclength( @@ -163,7 +163,7 @@ size_t getIdxByArclength( throw std::runtime_error("[getIdxByArclength] path points must be > 0"); } - using autoware_universe_utils::calcDistance2d; + using autoware::universe_utils::calcDistance2d; double sum_length = 0.0; if (signed_arc >= 0.0) { for (size_t i = target_idx; i < path.points.size() - 1; ++i) { @@ -185,7 +185,7 @@ size_t getIdxByArclength( return 0; } -// TODO(murooka) This function should be replaced with autoware_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) { @@ -202,7 +202,7 @@ void clipPathLength( path.points = clipped_points; } -// TODO(murooka) This function should be replaced with autoware_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) { @@ -412,7 +412,7 @@ std::vector dividePath( void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { - const auto is_driving_forward = autoware_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; @@ -472,7 +472,7 @@ std::vector interpolatePose( std::vector interpolated_poses{}; // output const std::vector base_s{ - 0, autoware_universe_utils::calcDistance2d(start_pose.position, end_pose.position)}; + 0, autoware::universe_utils::calcDistance2d(start_pose.position, end_pose.position)}; const std::vector base_x{start_pose.position.x, end_pose.position.x}; const std::vector base_y{start_pose.position.y, end_pose.position.y}; std::vector new_s; @@ -490,7 +490,7 @@ std::vector interpolatePose( std::sin(tf2::getYaw(end_pose.orientation)), new_s); for (size_t i = 0; i < interpolated_x.size(); ++i) { Pose pose{}; - pose = autoware_universe_utils::calcInterpolatedPose( + pose = autoware::universe_utils::calcInterpolatedPose( end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); pose.position.x = interpolated_x.at(i); pose.position.y = interpolated_y.at(i); @@ -509,14 +509,14 @@ Pose getUnshiftedEgoPose(const Pose & ego_pose, const ShiftedPath & prev_path) // un-shifted for current ideal pose const auto closest_idx = - autoware_motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); + 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 = - autoware_motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; + autoware::motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; - unshifted_pose = autoware_universe_utils::calcOffsetPose( + unshifted_pose = autoware::universe_utils::calcOffsetPose( unshifted_pose, 0.0, -prev_path.shift_length.at(closest_idx), 0.0); unshifted_pose.orientation = ego_pose.orientation; @@ -574,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 = autoware_motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); return filtered_path; } @@ -616,10 +616,10 @@ BehaviorModuleOutput getReferencePath( // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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 = 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 db41e59e63dbb..40802558c0b38 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 @@ -18,7 +18,7 @@ namespace autoware::behavior_path_planner::utils::traffic_light { -using autoware_motion_utils::calcSignedArcLength; +using autoware::motion_utils::calcSignedArcLength; double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) @@ -99,7 +99,7 @@ std::optional calcDistanceToRedTrafficLight( const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); return calcSignedArcLength( - path.points, ego_pos, autoware_universe_utils::createPoint(x, y, z)); + path.points, ego_pos, autoware::universe_utils::createPoint(x, y, z)); } } 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 c6a6256ef43c1..55a6c6ff39d30 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 @@ -43,11 +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 = autoware_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 = - autoware_motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + 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; @@ -62,7 +62,7 @@ double calcInterpolatedVelocity( const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { const double seg_dist = - autoware_motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + 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; @@ -73,10 +73,10 @@ double calcInterpolatedVelocity( namespace autoware::behavior_path_planner::utils { +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::Shape; -using autoware_universe_utils::LineString2d; -using autoware_universe_utils::Point2d; using geometry_msgs::msg::PoseWithCovarianceStamped; std::optional getPolygonByPoint( @@ -113,7 +113,7 @@ double getDistanceBetweenPredictedPaths( if (!ego_pose) { continue; } - double distance = autoware_universe_utils::calcDistance3d(*object_pose, *ego_pose); + double distance = autoware::universe_utils::calcDistance3d(*object_pose, *ego_pose); if (distance < min_distance) { min_distance = distance; } @@ -130,7 +130,7 @@ double getDistanceBetweenPredictedPathAndObject( double min_distance = std::numeric_limits::max(); rclcpp::Time ros_start_time = clock.now() + rclcpp::Duration::from_seconds(start_time); rclcpp::Time ros_end_time = clock.now() + rclcpp::Duration::from_seconds(end_time); - const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); for (double t = start_time; t < end_time; t += resolution) { const auto ego_pose = object_recognition_utils::calcInterpolatedPose(ego_path, t); if (!ego_pose) { @@ -147,7 +147,7 @@ double getDistanceBetweenPredictedPathAndObject( } bool checkCollisionBetweenPathFootprintsAndObjects( - const autoware_universe_utils::LinearRing2d & local_vehicle_footprint, + const autoware::universe_utils::LinearRing2d & local_vehicle_footprint, const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin) { for (const auto & p : ego_path.points) { @@ -160,14 +160,14 @@ bool checkCollisionBetweenPathFootprintsAndObjects( } bool checkCollisionBetweenFootprintAndObjects( - const autoware_universe_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const autoware::universe_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin) { const auto vehicle_footprint = - transformVector(local_vehicle_footprint, autoware_universe_utils::pose2transform(ego_pose)); + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); for (const auto & object : dynamic_objects.objects) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); if (distance < margin) return true; } @@ -178,18 +178,18 @@ double calcLateralDistanceFromEgoToObject( const Pose & ego_pose, const double vehicle_width, const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = autoware_universe_utils::toPolygon2d(dynamic_object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(dynamic_object); const auto vehicle_left_pose = - autoware_universe_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); const auto vehicle_right_pose = - autoware_universe_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); const double signed_distance_from_left = - autoware_universe_utils::calcLateralDeviation(vehicle_left_pose, point); + autoware::universe_utils::calcLateralDeviation(vehicle_left_pose, point); const double signed_distance_from_right = - autoware_universe_utils::calcLateralDeviation(vehicle_right_pose, point); + autoware::universe_utils::calcLateralDeviation(vehicle_right_pose, point); if (signed_distance_from_left < 0.0 && signed_distance_from_right > 0.0) { // point is between left and right @@ -208,21 +208,21 @@ double calcLongitudinalDistanceFromEgoToObject( const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = autoware_universe_utils::toPolygon2d(dynamic_object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(dynamic_object); const auto vehicle_front_pose = - autoware_universe_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); const auto vehicle_rear_pose = - autoware_universe_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); // forward is positive const double signed_distance_from_front = - autoware_universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point); + autoware::universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point); // backward is positive const double signed_distance_from_rear = - -autoware_universe_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); + -autoware::universe_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); if (signed_distance_from_front < 0.0 && signed_distance_from_rear < 0.0) { // point is between front and rear @@ -254,7 +254,7 @@ std::vector calcObjectsDistanceToPath( { std::vector distance_array; for (const auto & obj : objects.objects) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(obj); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj); LineString2d ego_path_line; ego_path_line.reserve(ego_path.points.size()); for (const auto & p : ego_path.points) { @@ -290,7 +290,7 @@ std::optional findIndexOutOfGoalSearchRange( const auto & lane_ids = points.at(i).lane_ids; const double dist_to_goal = - autoware_universe_utils::calcDistance2d(points.at(i).point.pose, goal); + autoware::universe_utils::calcDistance2d(points.at(i).point.pose, goal); const bool is_goal_lane_id_in_point = std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { @@ -307,7 +307,7 @@ std::optional findIndexOutOfGoalSearchRange( // find index out of goal search range size_t min_dist_out_of_range_index = min_dist_index; for (int i = min_dist_index; 0 <= i; --i) { - const double dist = autoware_universe_utils::calcDistance2d(points.at(i).point, goal); + const double dist = autoware::universe_utils::calcDistance2d(points.at(i).point, goal); min_dist_out_of_range_index = i; if (max_dist < dist) { break; @@ -343,7 +343,7 @@ bool setGoal( PathPointWithLaneId pre_refined_goal{}; constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = - autoware_universe_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); const size_t closest_seg_idx_for_pre_goal = findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = @@ -441,7 +441,7 @@ PathWithLaneId refinePathForGoal( { PathWithLaneId filtered_path = input; PathWithLaneId path_with_goal; - filtered_path.points = autoware_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()) { @@ -484,7 +484,7 @@ bool isInLaneletWithYawThreshold( const double pose_yaw = tf2::getYaw(current_pose.orientation); const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position); const double angle_diff = - std::abs(autoware_universe_utils::normalizeRadian(lanelet_angle - pose_yaw)); + std::abs(autoware::universe_utils::normalizeRadian(lanelet_angle - pose_yaw)); return (angle_diff < std::abs(yaw_threshold)) && lanelet::utils::isInLanelet(current_pose, lanelet, radius); @@ -511,7 +511,7 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - const double yaw_threshold = autoware_universe_utils::deg2rad(90); + const double yaw_threshold = autoware::universe_utils::deg2rad(90); if ( closest_road_lane.id() == goal_lane.id() && isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { @@ -564,7 +564,7 @@ bool isEgoWithinOriginalLane( const auto base_link2front = common_param.base_link2front; const auto base_link2rear = common_param.base_link2rear; const auto vehicle_width = common_param.vehicle_width; - const auto vehicle_poly = autoware_universe_utils::toFootprint( + const auto vehicle_poly = autoware::universe_utils::toFootprint( current_pose, base_link2front, base_link2rear, vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. @@ -782,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 = autoware_motion_utils::insertStopPoint(length, path.points); + const auto insert_idx = autoware::motion_utils::insertStopPoint(length, path.points); if (!insert_idx) { return PathPointWithLaneId(); } @@ -851,16 +851,16 @@ std::optional getSignedDistanceFromBoundary( rear_left.y = vehicle_width / 2; front_left.x = base_link2front; front_left.y = vehicle_width / 2; - rear_corner_point = autoware_universe_utils::transformPoint(rear_left, vehicle_pose); - front_corner_point = autoware_universe_utils::transformPoint(front_left, vehicle_pose); + rear_corner_point = autoware::universe_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = autoware::universe_utils::transformPoint(front_left, vehicle_pose); } else { Point front_right, rear_right; rear_right.x = -base_link2rear; rear_right.y = -vehicle_width / 2; front_right.x = base_link2front; front_right.y = -vehicle_width / 2; - rear_corner_point = autoware_universe_utils::transformPoint(rear_right, vehicle_pose); - front_corner_point = autoware_universe_utils::transformPoint(front_right, vehicle_pose); + rear_corner_point = autoware::universe_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = autoware::universe_utils::transformPoint(front_right, vehicle_pose); } const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); @@ -885,17 +885,17 @@ std::optional getSignedDistanceFromBoundary( const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); const Point inverse_p1 = - autoware_universe_utils::inverseTransformPoint(p1, vehicle_corner_pose); + autoware::universe_utils::inverseTransformPoint(p1, vehicle_corner_pose); const Point inverse_p2 = - autoware_universe_utils::inverseTransformPoint(p2, vehicle_corner_pose); + autoware::universe_utils::inverseTransformPoint(p2, vehicle_corner_pose); const double dx_p1 = inverse_p1.x; const double dx_p2 = inverse_p2.x; const double dy_p1 = inverse_p1.y; const double dy_p2 = inverse_p2.y; // Calculate the Euclidean distances between vehicle's corner and the current and next points. - const double distance1 = autoware_universe_utils::calcDistance2d(p1, vehicle_corner_point); - const double distance2 = autoware_universe_utils::calcDistance2d(p2, vehicle_corner_point); + const double distance1 = autoware::universe_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = autoware::universe_utils::calcDistance2d(p2, vehicle_corner_point); // If one of the bound points is behind and the other is in front of the vehicle corner point // and any of these points is closer than the current minimum distance, @@ -948,9 +948,9 @@ std::optional getSignedDistanceFromBoundary( bound_pose.orientation = vehicle_pose.orientation; const Point inverse_rear_point = - autoware_universe_utils::inverseTransformPoint(rear_corner_point, bound_pose); + autoware::universe_utils::inverseTransformPoint(rear_corner_point, bound_pose); const Point inverse_front_point = - autoware_universe_utils::inverseTransformPoint(front_corner_point, bound_pose); + autoware::universe_utils::inverseTransformPoint(front_corner_point, bound_pose); const double dx_rear = inverse_rear_point.x; const double dx_front = inverse_front_point.x; const double dy_rear = inverse_rear_point.y; @@ -1008,9 +1008,9 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) } polygon.outer().push_back(polygon.outer().front()); - return autoware_universe_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : autoware_universe_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) @@ -1021,8 +1021,9 @@ Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) } ret.outer().push_back(ret.outer().front()); - return autoware_universe_utils::isClockwise(ret) ? ret - : autoware_universe_utils::inverseClockwise(ret); + return autoware::universe_utils::isClockwise(ret) + ? ret + : autoware::universe_utils::inverseClockwise(ret); } std::vector getTargetLaneletPolygons( @@ -1150,13 +1151,13 @@ 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 = autoware_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 autoware_motion_utils::convertToRearWheelCenter( + return autoware::motion_utils::convertToRearWheelCenter( resampled_path_with_lane_id, rear_to_cog); } @@ -1196,7 +1197,7 @@ PathWithLaneId setDecelerationVelocity( for (auto & point : reference_path.points) { const auto arclength_to_target = std::max( - 0.0, autoware_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; @@ -1209,7 +1210,7 @@ PathWithLaneId setDecelerationVelocity( } const auto stop_point_length = - autoware_motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + + 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) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 543fbecd1b38b..85a80eac8f215 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -28,9 +28,9 @@ constexpr double epsilon = 1e-6; using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::Shape; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -47,8 +47,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); - ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -75,8 +75,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = autoware_universe_utils::createPoint(3.0, 4.0, 0.0); - ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware::universe_utils::createPoint(3.0, 4.0, 0.0); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -103,9 +103,9 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.position = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); ego_pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(autoware_universe_utils::deg2rad(60)); + autoware::universe_utils::createQuaternionFromYaw(autoware::universe_utils::deg2rad(60)); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -134,13 +134,13 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { using autoware::behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; - using autoware_universe_utils::createPoint; - using autoware_universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternionFromYaw; { Pose obj_pose; obj_pose.position = createPoint(0.0, 0.0, 0.0); - obj_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(0.0); + obj_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); Shape shape; shape.type = autoware_perception_msgs::msg::Shape::POLYGON; 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 342ebc985a518..f5996809dc057 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 @@ -26,9 +26,9 @@ using autoware::behavior_path_planner::PathWithLaneId; using autoware::behavior_path_planner::Pose; using autoware::behavior_path_planner::TurnSignalDecider; using autoware::behavior_path_planner::TurnSignalInfo; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::PathPoint; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromYaw; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; @@ -106,7 +106,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -118,7 +118,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -130,7 +130,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -142,7 +142,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -154,7 +154,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(48.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -166,7 +166,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(48.1f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -178,7 +178,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -190,7 +190,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -202,7 +202,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -214,7 +214,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) { Pose current_pose = generateEgoSamplePose(65.1f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -255,7 +255,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -267,7 +267,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) { Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -279,7 +279,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) { Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -291,7 +291,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -303,7 +303,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -315,7 +315,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) { Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -327,7 +327,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -369,7 +369,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -381,7 +381,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(29.9f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -393,7 +393,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(30.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -405,7 +405,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(33.9f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -417,7 +417,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(35.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -429,7 +429,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(44.9f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -441,7 +441,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -453,7 +453,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(49.9f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -465,7 +465,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, @@ -477,7 +477,7 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); const size_t current_seg_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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, 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 9ef227783bd9c..2b6f52998cb23 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 @@ -75,8 +75,8 @@ struct SamplingPlannerDebugData { std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; - std::vector obstacles{}; - std::vector footprints{}; + std::vector obstacles{}; + std::vector footprints{}; }; class SamplingPlannerModule : public SceneModuleInterface { @@ -204,10 +204,10 @@ class SamplingPlannerModule : public SceneModuleInterface if (length_to_goal < epsilon) return isReferencePathSafe(); const auto nearest_index = - autoware_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); + const auto rpy = autoware::universe_utils::getRPY(quat); return rpy.z; }; const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index 15a9e585a7690..f778eef981a92 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -21,11 +21,11 @@ #include namespace autoware::behavior_path_planner { -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::MultiPoint2d; -using autoware_universe_utils::MultiPolygon2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; struct SamplingPlannerParameters { // constraints.hard diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp index c43d1fe601b5f..4408645b26095 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp @@ -101,7 +101,7 @@ inline autoware::sampler_common::State getInitialState( { autoware::sampler_common::State initial_state; Point2d initial_state_pose{pose.position.x, pose.position.y}; - const auto rpy = autoware_universe_utils::getRPY(pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.orientation); initial_state.pose = initial_state_pose; initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y}); initial_state.heading = rpy.z; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp index 6ffd3ff116284..af8a538f894cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp @@ -77,7 +77,7 @@ void SamplingPlannerModuleManager::init(rclcpp::Node * node) void SamplingPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; 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 0e8e56489d6d4..526a761e6957a 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,13 +16,13 @@ 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 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; namespace bg = boost::geometry; @@ -103,7 +103,7 @@ SamplingPlannerModule::SamplingPlannerModule( // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = - // autoware_universe_utils::getRPY(input_data.goal_pose.orientation).z; const auto & + // autoware::universe_utils::getRPY(input_data.goal_pose.orientation).z; const auto & // last_point_yaw = path.yaws.back(); const double angle_difference = std::abs(last_point_yaw // - goal_pose_yaw); return angle_difference / (3.141519 / 4.0); // }); @@ -190,7 +190,7 @@ bool SamplingPlannerModule::isExecutionRequested() const return false; } - if (!autoware_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 = autoware_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; @@ -365,9 +365,9 @@ void SamplingPlannerModule::prepareConstraints( size_t i = 0; for (const auto & o : predicted_objects->objects) { if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) { - const auto polygon = autoware_universe_utils::toPolygon2d(o); + const auto polygon = autoware::universe_utils::toPolygon2d(o); constraints.obstacle_polygons.push_back(polygon); - const auto box = boost::geometry::return_envelope(polygon); + const auto box = boost::geometry::return_envelope(polygon); constraints.rtree.insert(std::make_pair(box, i)); } i++; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp index ddc9c820a6667..2e8d7e98c11cf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp @@ -48,7 +48,7 @@ void SideShiftModuleManager::init(rclcpp::Node * node) void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; [[maybe_unused]] auto p = parameters_; 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 065d15fbc8935..bcb972bf5dc8b 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,11 +28,11 @@ 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 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; SideShiftModule::SideShiftModule( @@ -196,7 +196,7 @@ void SideShiftModule::updateData() double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { max_dist = - std::max(max_dist, autoware_universe_utils::calcDistance2d(getEgoPose(), pnt.start)); + std::max(max_dist, autoware::universe_utils::calcDistance2d(getEgoPose(), pnt.start)); } return max_dist; }(); @@ -373,7 +373,8 @@ double SideShiftModule::getClosestShiftLength() const } const auto ego_point = planner_data_->self_odometry->pose.pose.position; - const auto closest = autoware_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 +397,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 = autoware_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); @@ -468,7 +469,7 @@ void SideShiftModule::setDebugMarkersVisualization() const debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { - autoware_universe_utils::appendMarkerArray(added, &debug_marker_); + autoware::universe_utils::appendMarkerArray(added, &debug_marker_); }; const auto add_shift_line_marker = [this, add]( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 52675653e7b32..7b30745057743 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -30,7 +30,7 @@ namespace autoware::behavior_path_planner { -using autoware_universe_utils::LinearRing2d; +using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 8be6bc911634a..137a2c53321fc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -81,7 +81,7 @@ std::optional FreespacePullOut::plan( const size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - autoware_universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); if (distance < th_end_distance) { last_path.points.erase(it, last_path.points.end()); break; 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 6e4928e0fbb7f..f3239d41bc1b9 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,9 +22,9 @@ #include -using autoware_motion_utils::findNearestIndex; -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::calcOffsetPose; +using autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; namespace autoware::behavior_path_planner { @@ -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 = - autoware_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 = - autoware_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 = autoware_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/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 0806978a7ef3f..9451bb3870648 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -351,7 +351,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; 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 c004596de6ff9..971c9a48e8b40 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 @@ -27,9 +27,9 @@ #include #include -using autoware_motion_utils::findNearestIndex; -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::calcOffsetPose; +using autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; namespace autoware::behavior_path_planner { @@ -108,7 +108,7 @@ std::optional ShiftPullOut::plan( // narrow place. const size_t start_segment_idx = - autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); @@ -125,17 +125,18 @@ 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 = - autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + 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 = - autoware_motion_utils::calcLongitudinalOffsetToSegment( + 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); + 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; }; @@ -174,7 +175,7 @@ bool ShiftPullOut::refineShiftedPathToStartPose( size_t iteration = 0; while (iteration < MAX_ITERATION) { const double lateral_offset = - autoware_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); @@ -196,7 +197,7 @@ bool ShiftPullOut::refineShiftedPathToStartPose( if (is_within_tolerance( lateral_offset, - autoware_motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), + autoware::motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), TOLERANCE)) { return true; } @@ -423,7 +424,7 @@ double ShiftPullOut::calcBeforeShiftedArcLength( double after_arc_length{0.0}; for (const auto & [k, segment_length] : - autoware_motion_utils::calcCurvatureAndArcLength(path.points)) { + 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 7c5030a9c69d3..f134e1645e9a5 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 @@ -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 autoware::motion_utils::calcLateralOffset; +using autoware::motion_utils::calcLongitudinalOffsetPose; +using autoware::universe_utils::calcOffsetPose; // set as macro so that calling function name will be printed. // debug print is heavy. turn on only when debugging. @@ -274,7 +274,7 @@ bool StartPlannerModule::hasFinishedBackwardDriving() const // check ego car is close enough to pull out start pose and stopped const auto current_pose = planner_data_->self_odometry->pose.pose; const auto distance = - autoware_universe_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); + autoware::universe_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); const bool is_near = distance < parameters_->th_arrived_distance; const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); @@ -383,7 +383,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { const double x = boundary_point.x(); const double y = boundary_point.y(); - boundary_path.push_back(autoware_universe_utils::createPoint(x, y, 0.0)); + boundary_path.push_back(autoware::universe_utils::createPoint(x, y, 0.0)); }); return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); @@ -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 = - autoware_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( + 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 = autoware_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; @@ -413,7 +413,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const const bool ego_is_merging_from_the_left) -> std::optional> { const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto vehicle_footprint = transformVector( - local_vehicle_footprint, autoware_universe_utils::pose2transform(current_pose)); + local_vehicle_footprint, autoware::universe_utils::pose2transform(current_pose)); double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits::max(); @@ -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 = autoware_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; @@ -524,7 +524,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - return autoware_universe_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( start_pose.position, planner_data_->self_odometry->pose.pose.position) > parameters_->th_arrived_distance; } @@ -532,7 +532,7 @@ bool StartPlannerModule::isCloseToOriginalStartPose() const bool StartPlannerModule::hasArrivedAtGoal() const { const Pose goal_pose = planner_data_->route_handler->getGoalPose(); - return autoware_universe_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( goal_pose.position, planner_data_->self_odometry->pose.pose.position) < parameters_->th_arrived_distance; } @@ -684,10 +684,10 @@ BehaviorModuleOutput StartPlannerModule::plan() const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { - const double start_distance = autoware_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 = autoware_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 = autoware_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 = autoware_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 = autoware_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 = autoware_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); @@ -913,7 +913,7 @@ bool StartPlannerModule::findPullOutPath( // if start_pose_candidate is far from refined_start_pose, backward driving is necessary constexpr double epsilon = 0.01; const double backwards_distance = - autoware_universe_utils::calcDistance2d(start_pose_candidate, refined_start_pose); + autoware::universe_utils::calcDistance2d(start_pose_candidate, refined_start_pose); const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); @@ -1240,7 +1240,7 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( bool StartPlannerModule::hasReachedFreespaceEnd() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; - return autoware_universe_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + return autoware::universe_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < parameters_->th_arrived_distance; } @@ -1282,8 +1282,8 @@ bool StartPlannerModule::hasFinishedCurrentPath() const auto current_path = getCurrentPath(); const auto current_path_end = current_path.points.back(); const auto self_pose = planner_data_->self_odometry->pose.pose; - const bool is_near_target = autoware_universe_utils::calcDistance2d(current_path_end, self_pose) < - parameters_->th_arrived_distance; + const bool is_near_target = autoware::universe_utils::calcDistance2d( + current_path_end, self_pose) < parameters_->th_arrived_distance; return is_near_target && isStopped(); } @@ -1294,10 +1294,10 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto shift_start_idx = - autoware_motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); + const auto shift_start_idx = autoware::motion_utils::findNearestIndex( + path.points, status_.pull_out_path.start_pose.position); const auto shift_end_idx = - autoware_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) { @@ -1337,7 +1337,7 @@ 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(autoware_motion_utils::calcSignedArcLength( + 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; }); @@ -1450,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 = autoware_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); @@ -1563,9 +1563,9 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() { - using autoware_universe_utils::createDefaultMarker; - using autoware_universe_utils::createMarkerColor; - using autoware_universe_utils::createMarkerScale; + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; @@ -1589,7 +1589,7 @@ void StartPlannerModule::setDebugData() for (auto & marker : added.markers) { marker.lifetime = life_time; } - autoware_universe_utils::appendMarkerArray(added, &target_marker_array); + autoware::universe_utils::appendMarkerArray(added, &target_marker_array); }; debug_marker_.markers.clear(); @@ -1619,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 = autoware_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) { @@ -1673,9 +1673,9 @@ 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 = autoware_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 = autoware_motion_utils::findNearestIndex( + 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( 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 82eadd656ac30..3fe9524e34a17 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 @@ -65,7 +65,7 @@ PathWithLaneId getBackwardPath( const double lateral_distance_to_shoulder_center = current_pose_arc_coords.distance; for (size_t i = 0; i < backward_path.points.size(); ++i) { auto & p = backward_path.points.at(i).point.pose; - p = autoware_universe_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); + p = autoware::universe_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); } } @@ -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 = - autoware_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 = autoware_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 - ? autoware_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 04870464c2777..8c46affbc64e3 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 = autoware_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 = autoware_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 = autoware_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 = autoware_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,14 +449,14 @@ 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 = autoware_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 = autoware_motion_utils::calcSignedArcLength( + 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; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 0000790a3adde..361fe60c21eb1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -26,8 +26,8 @@ namespace autoware::behavior_path_planner { +using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; -using autoware_universe_utils::getOrDeclareParameter; AvoidanceParameters getParameter(rclcpp::Node * node) { 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 ecd83c31f32c8..f1769a45be530 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,7 +115,7 @@ 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 = autoware_motion_utils::calcSignedArcLength( + 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( @@ -128,7 +128,7 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } for (const auto & right_shift : right_shift_array_) { - const double start_distance = autoware_motion_utils::calcSignedArcLength( + 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( 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 0ac860fc41882..cfbd3f89308ac 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 @@ -53,22 +53,22 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_rtc_msgs::msg::State; // tier4 utils functions -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::calcLateralDeviation; -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::calcYawDeviation; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; -using autoware_universe_utils::createPoint; -using autoware_universe_utils::createQuaternionFromRPY; -using autoware_universe_utils::getPoint; -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 autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcLateralDeviation; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::calcYawDeviation; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::getPoint; +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; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 6b28e33759b6a..03b17e7729ed9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -691,7 +691,7 @@ std::string toStrInfo(const autoware::behavior_path_planner::AvoidLineArray & ap } std::string toStrInfo(const autoware::behavior_path_planner::AvoidLine & ap) { - using autoware_universe_utils::toHexString; + using autoware::universe_utils::toHexString; std::stringstream pids; for (const auto pid : ap.parent_ids) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 0ca2b9c4e0d21..2d02f33e19870 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -28,8 +28,8 @@ namespace autoware::behavior_path_planner { void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { + using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; - using autoware_universe_utils::getOrDeclareParameter; // init manager interface initInterface(node, {"left", "right"}); @@ -42,8 +42,8 @@ void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void StaticObstacleAvoidanceModuleManager::updateModuleParams( const std::vector & parameters) { + using autoware::universe_utils::updateParam; using autoware_perception_msgs::msg::ObjectClassification; - using autoware_universe_utils::updateParam; auto p = parameters_; 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 6600846ce04a1..bbdb8078aadf9 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(), - autoware_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 = - autoware_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 = autoware_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_ = autoware_motion_utils::calcLongitudinalOffsetPose( + dead_pose_ = autoware::motion_utils::calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); if (!dead_pose_) { @@ -755,7 +755,7 @@ bool StaticObstacleAvoidanceModule::isSafePath( auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = - autoware_universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); + autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); @@ -812,7 +812,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( return a.start_idx < b.start_idx; }); return std::max( - max_dist, autoware_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 = autoware_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 > - autoware_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 = - autoware_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 = autoware_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 = - autoware_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,7 +1602,7 @@ void StaticObstacleAvoidanceModule::insertStopPoint( return shifted_path.path.points.size() - 1; }(); - const auto stop_distance = autoware_motion_utils::calcSignedArcLength( + 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 @@ -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 = - autoware_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_ = autoware_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 = - autoware_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_ = autoware_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 7580623cff714..a171ae9161041 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,7 +355,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return false; } const auto goal_pose = data_->route_handler->getGoalPose(); - const double goal_longitudinal_distance = autoware_motion_utils::calcSignedArcLength( + 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) < @@ -365,7 +365,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; const bool has_object_near_goal = - autoware_universe_utils::calcDistance2d(goal_pose.position, object_pos) < + autoware::universe_utils::calcDistance2d(goal_pose.position, object_pos) < parameters_->object_check_goal_distance; return has_object_near_goal; }(); @@ -1026,7 +1026,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const auto has_object_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return autoware_universe_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( data_->route_handler->getGoalPose().position, o.object.kinematics.initial_pose_with_covariance.pose.position) < parameters_->object_check_goal_distance; @@ -1097,7 +1097,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const bool has_last_shift_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return autoware_universe_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( last_sl.end.position, o.object.kinematics.initial_pose_with_covariance.pose.position) < parameters_->object_check_goal_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 2704829ad6a11..05561318039b7 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 @@ -56,7 +56,7 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const } geometry_msgs::msg::Polygon toMsg( - const autoware_universe_utils::Polygon2d & polygon, const double z) + const autoware::universe_utils::Polygon2d & polygon, const double z) { geometry_msgs::msg::Polygon ret; for (const auto & p : polygon.outer()) { @@ -68,14 +68,14 @@ geometry_msgs::msg::Polygon toMsg( template size_t findFirstNearestIndex(const T & points, const geometry_msgs::msg::Point & point) { - autoware_motion_utils::validateNonEmpty(points); + autoware::motion_utils::validateNonEmpty(points); double min_dist = std::numeric_limits::max(); size_t min_idx = 0; bool decreasing = false; for (size_t i = 0; i < points.size(); ++i) { - const auto dist = autoware_universe_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { decreasing = true; min_dist = dist; @@ -104,7 +104,7 @@ size_t findFirstNearestSegmentIndex(const T & points, const geometry_msgs::msg:: } const double signed_length = - autoware_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 { - autoware_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 = - autoware_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 = - autoware_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 = - autoware_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; } @@ -329,7 +329,7 @@ bool isWithinIntersection( return false; } - const auto object_polygon = autoware_universe_utils::toPolygon2d(object.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); @@ -646,7 +646,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = autoware_universe_utils::toPolygon2d(object.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); const auto is_disjoint_right_lane = boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon()); if (is_disjoint_right_lane) { @@ -669,7 +669,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = autoware_universe_utils::toPolygon2d(object.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); const auto is_disjoint_left_lane = boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon()); if (is_disjoint_left_lane) { @@ -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()) - ? autoware_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(); @@ -941,12 +941,12 @@ double getRoadShoulderDistance( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data) { - using autoware_universe_utils::Point2d; + using autoware::universe_utils::Point2d; using lanelet::utils::to2D; const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - autoware_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; @@ -969,7 +969,7 @@ double getRoadShoulderDistance( const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; const auto opt_intersect = - autoware_universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware::universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( @@ -987,7 +987,7 @@ double getRoadShoulderDistance( calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) .position; const auto opt_intersect = - autoware_universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware::universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( @@ -1036,8 +1036,8 @@ bool isWithinLanes( { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto transform = autoware_universe_utils::pose2transform(ego_pose); - const auto footprint = autoware_universe_utils::transformVector( + const auto transform = autoware::universe_utils::pose2transform(ego_pose); + const auto footprint = autoware::universe_utils::transformVector( planner_data->parameters.vehicle_info.createFootprint(), transform); lanelet::ConstLanelet closest_lanelet{}; @@ -1193,10 +1193,10 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( double min_distance = std::numeric_limits::max(); double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { - const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); + 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 = - autoware_motion_utils::calcSignedArcLength(path.points, ego_pos, point); + 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); } @@ -1211,9 +1211,9 @@ std::vector> calcEnvelopeOverhangDistance( std::vector> overhang_points{}; for (const auto & p : object_data.envelope_poly.outer()) { - const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); + 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 = autoware_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); } @@ -1247,9 +1247,9 @@ Polygon2d createEnvelopePolygon( const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer) { namespace bg = boost::geometry; - using autoware_universe_utils::expandPolygon; - using autoware_universe_utils::Point2d; - using autoware_universe_utils::Polygon2d; + using autoware::universe_utils::expandPolygon; + using autoware::universe_utils::Point2d; + using autoware::universe_utils::Polygon2d; using Box = bg::model::box; const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) { @@ -1293,7 +1293,7 @@ Polygon2d createEnvelopePolygon( Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) { - const auto object_polygon = autoware_universe_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object_data.object); return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer); } @@ -1325,7 +1325,7 @@ std::vector generateObstaclePolygonsForDrivableArea( const double diff_poly_buffer = object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = - autoware_universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); + autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); obstacles_for_drivable_area.push_back( {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); } @@ -1385,7 +1385,7 @@ void insertDecelPoint( std::optional & p_out) { const auto decel_point = - autoware_motion_utils::calcLongitudinalOffsetPoint(path.points, p_src, offset); + 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. @@ -1393,9 +1393,9 @@ void insertDecelPoint( } const auto seg_idx = - autoware_motion_utils::findNearestSegmentIndex(path.points, decel_point.value()); + autoware::motion_utils::findNearestSegmentIndex(path.points, decel_point.value()); const auto insert_idx = - autoware_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. @@ -1457,7 +1457,7 @@ void fillObjectEnvelopePolygon( const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); - const auto object_polygon = autoware_universe_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object_data.object); const auto object_polygon_area = boost::geometry::area(object_polygon); const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly); @@ -1817,9 +1817,9 @@ void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineAr // calc longitudinal for (auto & sl : lines) { - sl.start_idx = autoware_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 = autoware_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); } } @@ -1868,7 +1868,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( { // TODO(Horibe) parametrize const auto isSimilar = [](const AvoidLine & a, const AvoidLine & b) { - using autoware_universe_utils::calcDistance2d; + using autoware::universe_utils::calcDistance2d; if (calcDistance2d(a.start, b.start) > 1.0) { return false; } @@ -2117,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 = - autoware_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; } @@ -2155,7 +2155,7 @@ std::pair separateObjectsByPath( const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { @@ -2365,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 = - autoware_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/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp index 31d90a7d7d708..714280daad38e 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 @@ -27,10 +27,10 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; namespace { @@ -73,12 +73,12 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( } // namespace -autoware_motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWalls virtual_walls; if (debug_data_.virtual_wall_pose) { - autoware_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 e4b3a341a9b91..da4582dd1643f 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 @@ -88,7 +88,7 @@ void BlindSpotModule::setRTCStatusByDecision( { setSafe(false); const auto & current_pose = planner_data_->current_odometry->pose; - setDistance(autoware_motion_utils::calcSignedArcLength( + setDistance(autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, decision.stop_line_idx)); return; } @@ -124,7 +124,7 @@ void BlindSpotModule::setRTCStatusByDecision( { setSafe(true); const auto & current_pose = planner_data_->current_odometry->pose; - setDistance(autoware_motion_utils::calcSignedArcLength( + 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 e51a55784b72c..f7e859a9085c7 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 @@ -91,7 +91,7 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( /* calc closest index */ const auto & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx = autoware_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 = @@ -256,7 +256,7 @@ std::optional BlindSpotModule::getSiblingStraightLanelet( static std::optional getFirstPointIntersectsLineByFootprint( const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -266,8 +266,8 @@ static std::optional getFirstPointIntersectsLineByFootprint( const auto line2d = line.basicLineString(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware_universe_utils::transformVector( - footprint, autoware_universe_utils::pose2transform(base_pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, line2d)) { return std::make_optional(i); } @@ -282,7 +282,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (autoware_universe_utils::calcDistance2d(p, point) < min_dist) { + if (autoware::universe_utils::calcDistance2d(p, point) < min_dist) { return i; } } @@ -299,7 +299,7 @@ static std::optional insertPointIndex( return duplicate_idx_opt.value(); } - const size_t closest_idx = autoware_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 = - autoware_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(autoware_motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - + static_cast(autoware::motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - baselink2front_dist)); */ } @@ -420,12 +420,12 @@ std::optional BlindSpotModule::isOverPassJudge( planner_data_->current_velocity->twist.linear.x, planner_data_->max_stop_acceleration_threshold, planner_data_->delay_response_time); const auto ego_segment_idx = - autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + 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 = - autoware_motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); - const auto distance_until_stop = autoware_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 8c4cae6ba3993..7faf78cb2d979 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/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp index 8e8686e984a40..d96256d2753ce 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 @@ -24,14 +24,14 @@ 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 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 visualization_msgs::msg::Marker; namespace @@ -177,19 +177,19 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( } } // namespace -autoware_motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_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 = autoware_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 = autoware_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/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 398368ccd51e7..82cddc2c28f1e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::Crosswalk; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 444dd68767919..55aa4daed9f87 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -118,7 +118,7 @@ void clear_occlusions_behind_objects( object.kinematics.initial_pose_with_covariance.pose.position.y}; if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; - const auto object_polygon = autoware_universe_utils::toPolygon2d(object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); std::sort(edge_points.begin(), edge_points.end(), angle_cmp); // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); 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 631f08ce8494e..786828d72dcb3 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 @@ -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 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; namespace { @@ -85,7 +85,7 @@ void offsetPolygon2d( { for (const auto & polygon_point : polygon.points) { const auto offset_pos = - autoware_universe_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + autoware::universe_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) .position; offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } @@ -99,7 +99,7 @@ Polygon2d createMultiStepPolygon( Polygon2d multi_step_polygon{}; for (size_t i = start_idx; i <= end_idx; ++i) { offsetPolygon2d( - autoware_universe_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); + autoware::universe_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); } Polygon2d hull_multi_step_polygon{}; @@ -385,9 +385,10 @@ 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 = 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 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; if ( dist_ego2crosswalk - base_link2front - braking_distance < @@ -414,7 +415,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( if ( isVehicleType(object.classification) && ego_crosswalk_passage_direction && collision_point.crosswalk_passage_direction) { - const double direction_diff = autoware_universe_utils::normalizeRadian( + const double direction_diff = autoware::universe_utils::normalizeRadian( collision_point.crosswalk_passage_direction.value() - ego_crosswalk_passage_direction.value()); if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { @@ -648,14 +649,14 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional> { const auto line_start = - autoware_universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - autoware_universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + autoware::universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); for (unsigned i = 0; i < path.points.size() - 1; ++i) { const auto & start = path.points.at(i).point.pose.position; const auto & end = path.points.at(i + 1).point.pose.position; if (const auto intersect = - autoware_universe_utils::intersect(line_start, line_end, start, end); + autoware::universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return std::make_optional(std::make_pair(i, intersect.value())); } @@ -677,19 +678,19 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( const autoware_perception_msgs::msg::PredictedPath & path) const { - using autoware_universe_utils::Segment2d; + using autoware::universe_utils::Segment2d; auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional> { const auto line_start = - autoware_universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - autoware_universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + autoware::universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); for (unsigned i = 0; i < path.path.size() - 1; ++i) { const auto & start = path.path.at(i).position; const auto & end = path.path.at(i + 1).position; if (const auto intersect = - autoware_universe_utils::intersect(line_start, line_end, start, end); + autoware::universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return std::make_optional(std::make_pair(i, intersect.value())); } 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 8d53b0a1fe4fd..491e0dadd4d80 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 @@ -49,12 +49,12 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::StopWatch; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; -using autoware_universe_utils::Polygon2d; -using autoware_universe_utils::StopWatch; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_planning_msgs::msg::PathWithLaneId; @@ -264,7 +264,7 @@ class CrosswalkModule : public SceneModuleInterface const bool is_object_away_from_path = !attention_area.outer().empty() && boost::geometry::distance( - autoware_universe_utils::fromMsg(position).to_2d(), attention_area) > 0.5; + autoware::universe_utils::fromMsg(position).to_2d(), attention_area) > 0.5; // add new object if (objects.count(uuid) == 0) { @@ -327,7 +327,7 @@ class CrosswalkModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - autoware_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 c2d9f8273cabe..93b9780445479 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 @@ -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 autoware::motion_utils::calcSignedArcLength; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::Line2d; +using autoware::universe_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, @@ -152,7 +152,7 @@ std::vector getPolygonIntersects( std::sort(intersects.begin(), intersects.end(), compare); - // convert autoware_universe_utils::Point2d to geometry::msg::Point + // convert autoware::universe_utils::Point2d to geometry::msg::Point std::vector geometry_points; for (const auto & p : intersects) { geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); @@ -201,7 +201,7 @@ std::vector getLinestringIntersects( std::sort(intersects.begin(), intersects.end(), compare); - // convert autoware_universe_utils::Point2d to geometry::msg::Point + // convert autoware::universe_utils::Point2d to geometry::msg::Point std::vector geometry_points; for (const auto & p : intersects) { geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); 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 4eff964f96c60..87df2a620f717 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 @@ -31,10 +31,10 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; using std_msgs::msg::ColorRGBA; namespace @@ -171,21 +171,21 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray return wall_marker; } -autoware_motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; wall.text = "detection_area"; - wall.style = autoware_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); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } - wall.style = autoware_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); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 6bd0d2c3b274a..4887fd7423817 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::DetectionArea; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) 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 954bbc988b1c3..7abd88d59f398 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 @@ -33,8 +33,8 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_motion_utils::calcLongitudinalOffsetPose; -using autoware_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, @@ -288,7 +288,7 @@ std::vector DetectionAreaModule::getObstaclePoints() (circle.first.y() - p.y) * (circle.first.y() - p.y); if (squared_dist <= circle.second) { if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) { - obstacle_points.push_back(autoware_universe_utils::createPoint(p.x, p.y, p.z)); + obstacle_points.push_back(autoware::universe_utils::createPoint(p.x, p.y, p.z)); // get all obstacle point becomes high computation cost so skip if any point is found break; } 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 595ab663f8b01..5b15cc92f57b1 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; - autoware_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/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index 1543966e47276..a53b6e7cd7f92 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 @@ -33,10 +33,10 @@ namespace { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, @@ -226,10 +226,10 @@ constexpr std::tuple light_blue() namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { @@ -424,27 +424,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( return debug_marker_array; } -autoware_motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; if (debug_data_.collision_stop_wall_pose) { - wall.style = autoware_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 = autoware_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 = autoware_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 @@ autoware_motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() virtual_walls.push_back(wall); } if (debug_data_.absence_traffic_light_creep_wall) { - wall.style = autoware_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; } -autoware_motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWalls virtual_walls; const auto state = state_machine_.getState(); if (state == StateMachine::State::STOP) { - autoware_motion_utils::VirtualWall wall; - wall.style = autoware_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/intersection_lanelets.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index 470464a6156b2..648fc611ac6b1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner void IntersectionLanelets::update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr) { is_prioritized_ = is_prioritized; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index af11033a454a7..f8110475e2782 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -41,7 +41,7 @@ struct IntersectionLanelets */ void update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr); const lanelet::ConstLanelets & attention() const diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 5ec002535f1a1..3fa2dc3efb5b1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index cc4b7cb0658be..650363c602b90 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -38,15 +38,15 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) return ss.str(); } -autoware_universe_utils::Polygon2d createOneStepPolygon( +autoware::universe_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, const autoware_perception_msgs::msg::Shape & shape) { namespace bg = boost::geometry; - const auto prev_poly = autoware_universe_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = autoware_universe_utils::toPolygon2d(next_pose, shape); + const auto prev_poly = autoware::universe_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = autoware::universe_utils::toPolygon2d(next_pose, shape); - autoware_universe_utils::Polygon2d one_step_poly; + autoware::universe_utils::Polygon2d one_step_poly; for (const auto & point : prev_poly.outer()) { one_step_poly.outer().push_back(point); } @@ -56,7 +56,7 @@ autoware_universe_utils::Polygon2d createOneStepPolygon( bg::correct(one_step_poly); - autoware_universe_utils::Polygon2d convex_one_step_poly; + autoware::universe_utils::Polygon2d convex_one_step_poly; bg::convex_hull(one_step_poly, convex_one_step_poly); return convex_one_step_poly; @@ -164,13 +164,13 @@ bool ObjectInfo::can_stop_before_ego_lane( const auto stopline = stopline_opt.value(); const auto stopline_p1 = stopline.front(); const auto stopline_p2 = stopline.back(); - const autoware_universe_utils::Point2d stopline_mid{ + const autoware::universe_utils::Point2d stopline_mid{ (stopline_p1.x() + stopline_p2.x()) / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; const auto attention_lane_end = attention_lanelet.centerline().back(); - const autoware_universe_utils::LineString2d attention_lane_later_part( - {autoware_universe_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, - autoware_universe_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); - std::vector ego_collision_points; + const autoware::universe_utils::LineString2d attention_lane_later_part( + {autoware::universe_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + autoware::universe_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; bg::intersection( attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); if (ego_collision_points.empty()) { 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 3a760d404c80b..274bbe5627b22 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 @@ -42,7 +42,7 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_motion_utils::VelocityFactorInterface; +using autoware::motion_utils::VelocityFactorInterface; IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, @@ -56,7 +56,7 @@ IntersectionModule::IntersectionModule( associative_ids_(associative_ids), turn_direction_(turn_direction), has_traffic_light_(has_traffic_light), - occlusion_uuid_(autoware_universe_utils::generateUUID()) + occlusion_uuid_(autoware::universe_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -342,7 +342,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail( // Occluded // utility functions auto fromEgoDist = [&](const size_t index) { - return autoware_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) { @@ -528,12 +528,12 @@ void prepareRTCByDecisionResult( const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; *default_distance = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); + 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 = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); } return; } @@ -549,7 +549,7 @@ void prepareRTCByDecisionResult( const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; *default_distance = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); *occlusion_safety = true; return; } @@ -565,11 +565,11 @@ void prepareRTCByDecisionResult( const auto collision_stopline_idx = result.collision_stopline_idx; *default_safety = false; *default_distance = - autoware_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 = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline); return; } @@ -585,10 +585,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - autoware_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 = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -604,10 +604,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - autoware_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 = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -622,7 +622,7 @@ void prepareRTCByDecisionResult( const auto collision_stopline_idx = result.closest_idx; *default_safety = !result.collision_detected; *default_distance = - autoware_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; @@ -640,10 +640,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - autoware_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 = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -658,10 +658,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - autoware_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 = - autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -677,10 +677,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = !result.collision_detected; *default_distance = - autoware_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 = - autoware_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 6aa81162bea25..ea2e1a7ae16d7 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 @@ -325,7 +325,7 @@ class IntersectionModule : public SceneModuleInterface /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - autoware_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 041715b2dc1a9..739b1122be6a1 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 @@ -111,7 +111,7 @@ void IntersectionModule::updateObjectInfoManagerArea() return false; } return bg::within( - autoware_universe_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + autoware::universe_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); }(); std::optional attention_lanelet{std::nullopt}; std::optional stopline{std::nullopt}; @@ -311,7 +311,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( for (auto i = begin; i <= end; ++i) { if (bg::intersects( polygon, - autoware_universe_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + autoware::universe_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { collision_detected = true; break; } @@ -490,13 +490,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_1st_judge_line_pose.position.x, // 4 - passed_1st_judge_line_pose.position.y, // 5 - autoware_universe_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + autoware::universe_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 ); } } @@ -528,13 +528,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_2nd_judge_line_pose.position.x, // 4 - passed_2nd_judge_line_pose.position.y, // 5 - autoware_universe_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + autoware::universe_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 ); } } @@ -614,10 +614,10 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( const auto [begin, end] = unsafe_interval.interval_position; const auto &p1 = unsafe_interval.path.at(begin).position, p2 = unsafe_interval.path.at(end).position; - const auto collision_pos = - autoware_universe_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto collision_pos = autoware::universe_utils::createPoint( + (p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); const auto object_dist_to_margin_point = - autoware_universe_utils::calcDistance2d( + autoware::universe_utils::calcDistance2d( object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, collision_pos) - planner_param_.collision_detection.avoid_collision_by_acceleration @@ -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 = - autoware_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; }); @@ -780,7 +780,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); const double angle_diff = - autoware_universe_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + autoware::universe_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return std::make_optional(i); @@ -898,7 +898,7 @@ 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 = - autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + 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); @@ -906,7 +906,7 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin if (upstream_stopline) { const auto upstream_stopline_point = reference_path.points.at(upstream_stopline.value()).point.pose; - return autoware_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 { @@ -918,7 +918,7 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin const auto & p1 = smoothed_reference_path.points.at(i); const auto & p2 = smoothed_reference_path.points.at(i + 1); - const double dist = autoware_universe_utils::calcDistance2d(p1, p2); + const double dist = autoware::universe_utils::calcDistance2d(p1, p2); dist_sum += dist; // use average velocity between p1 and p2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index f05b49a52b165..24e9a3b4a1793 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -151,7 +151,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( auto findCommonCvPolygons = [&](const auto & area2d, std::vector> & cv_polygons) -> void { - autoware_universe_utils::Polygon2d area2d_poly; + autoware::universe_utils::Polygon2d area2d_poly; for (const auto & p : area2d) { area2d_poly.outer().emplace_back(p.x(), p.y()); } @@ -240,7 +240,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( std::vector> blocking_polygons; for (const auto & blocking_attention_object_info : blocking_attention_objects) { const Polygon2d obj_poly = - autoware_universe_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); + autoware::universe_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -321,7 +321,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { const auto path_linestring_point = path_ip.points.at(i).point.pose.position; if ( - autoware_universe_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + autoware::universe_utils::calcDistance2d( + prev_path_linestring_point, path_linestring_point) < 1.0 /* rough tick for computational cost */) { continue; } @@ -405,9 +406,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( division_index, std::distance(division.begin(), point_it), acc_dist, - autoware_universe_utils::createPoint(point_it->x(), point_it->y(), origin.z), - autoware_universe_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), - autoware_universe_utils::createPoint( + autoware::universe_utils::createPoint(point_it->x(), point_it->y(), origin.z), + autoware::universe_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), + autoware::universe_utils::createPoint( projection_it->x(), projection_it->y(), origin.z) /* initialize with projection point at first*/}; found_min_dist_for_this_division = true; @@ -416,7 +417,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // division previously in this iteration, and the iterated cells are still OCCLUDED since // then nearest_occlusion_point.visible_end = - autoware_universe_utils::createPoint(point_it->x(), point_it->y(), origin.z); + autoware::universe_utils::createPoint(point_it->x(), point_it->y(), origin.z); } } is_prev_occluded = (pixel == OCCLUDED); @@ -440,7 +441,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { const auto obj_poly = - autoware_universe_utils::toPolygon2d(attention_object_info->predicted_object()); + autoware::universe_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_triangle)) { debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; 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 42d91defe7569..1390104d98175 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 @@ -31,7 +31,7 @@ #include #include -namespace autoware_universe_utils +namespace autoware::universe_utils { template <> @@ -44,7 +44,7 @@ inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) return point; } -} // namespace autoware_universe_utils +} // namespace autoware::universe_utils namespace { @@ -74,7 +74,7 @@ lanelet::ConstLanelet generatePathLanelet( for (size_t i = start_idx; i <= end_idx; ++i) { const auto & p = path.points.at(i).point.pose; const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && autoware_universe_utils::calcDistance2d(p_prev, p) < interval) { + if (i != start_idx && autoware::universe_utils::calcDistance2d(p_prev, p) < interval) { continue; } prev_idx = i; @@ -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 = autoware_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,9 +275,9 @@ 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 = - autoware_motion_utils::calcSignedArcLength( + autoware::motion_utils::calcSignedArcLength( path->points, closest_idx, - autoware_universe_utils::createPoint( + autoware::universe_utils::createPoint( assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), assigned_lane_begin_point.z())) < planner_param_.collision_detection.yield_on_green_traffic_light @@ -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 autoware_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); } @@ -381,8 +381,8 @@ std::optional IntersectionModule::generateIntersectionSto std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware_universe_utils::transformVector( - local_footprint, autoware_universe_utils::pose2transform(base_pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + local_footprint, autoware::universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { // NOTE: maybe consideration of braking dist is necessary first_footprint_attention_centerline_ip_opt = i; @@ -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 = autoware_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); @@ -423,8 +423,8 @@ std::optional IntersectionModule::generateIntersectionSto // NOTE: if footprints[0] is already inside the attention area, invalid { const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = autoware_universe_utils::transformVector( - local_footprint, autoware_universe_utils::pose2transform(base_pose0)); + const auto path_footprint0 = autoware::universe_utils::transformVector( + local_footprint, autoware::universe_utils::pose2transform(base_pose0)); if (bg::intersects( path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { occlusion_peeking_line_valid = false; 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 124f679e00a5d..14baf6faa581a 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 @@ -124,7 +124,7 @@ std::optional IntersectionModule::isStuckStatus( { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { - return autoware_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 @@ -290,7 +290,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & pa } // check if the footprint is in the stuck detect area - const auto obj_footprint = autoware_universe_utils::toPolygon2d(object); + const auto obj_footprint = autoware::universe_utils::toPolygon2d(object); // NOTE: in order not to stop too much const bool is_in_stuck_area = bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), @@ -310,7 +310,7 @@ std::optional IntersectionModule::isYieldStuckStatus( { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { - return autoware_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 dcbd41eccaec8..ddfd3b7d4cacd 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 @@ -53,7 +53,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); @@ -63,8 +63,8 @@ static std::optional getFirstConflictingLanelet( for (size_t i = start; i <= end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware_universe_utils::transformVector( - footprint, autoware_universe_utils::pose2transform(pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(pose)); for (const auto & conflicting_lanelet : conflicting_lanelets) { const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); const bool intersects = bg::intersects(polygon_2d, path_footprint); @@ -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 = autoware_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 4960c7ab9e8d5..a69297adea881 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; - autoware_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 e56b99486c93c..893ec646dba73 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 @@ -51,7 +51,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (autoware_universe_utils::calcDistance2d(p, point) < min_dist) { + if (autoware::universe_utils::calcDistance2d(p, point) < min_dist) { return i; } } @@ -68,7 +68,7 @@ std::optional insertPointIndex( return duplicate_idx_opt.value(); } - const size_t closest_idx = autoware_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) @@ -130,7 +130,7 @@ std::optional> findLaneIdsInterval( std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -140,8 +140,8 @@ std::optional getFirstPointInsidePolygonByFootprint( const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware_universe_utils::transformVector( - footprint, autoware_universe_utils::pose2transform(base_pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, area_2d)) { return std::make_optional(i); } @@ -154,7 +154,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -164,8 +164,8 @@ getFirstPointInsidePolygonsByFootprint( for (size_t i = start; i <= lane_end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware_universe_utils::transformVector( - footprint, autoware_universe_utils::pose2transform(pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(pose)); for (size_t j = 0; j < polygons.size(); ++j) { const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); const bool is_in_polygon = bg::intersects(area_2d, path_footprint); @@ -332,7 +332,7 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp index 1907395b4baee..f89d6b0ea67ae 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -87,7 +87,7 @@ bool isBeforeTargetIndex( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** @@ -125,7 +125,7 @@ mergeLaneletsByTopologicalSort( */ std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); /** * @brief find the index of the first point where vehicle footprint intersects with the given @@ -136,7 +136,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); std::vector getPolygon3dFromLanelets( const lanelet::ConstLanelets & ll_vec); 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 b587036d9027a..09f1908b107d5 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 @@ -34,10 +34,10 @@ namespace { const double marker_lifetime = 0.2; using DebugData = NoStoppingAreaModule::DebugData; -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) { @@ -162,15 +162,15 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArra return debug_marker_array; } -autoware_motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_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 = autoware_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); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 773ec5f44411e..4ee711297b494 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::NoStoppingArea; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) 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 18169dfa1ba70..d007416dccb47 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 @@ -87,7 +87,7 @@ boost::optional NoStoppingAreaModule::getStopLineGeometry2d( std::vector collision_points; bg::intersection(area_poly, line, collision_points); if (!collision_points.empty()) { - const double yaw = autoware_universe_utils::calcAzimuthAngle(p0, p1); + const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); const double w = planner_data_->vehicle_info_.vehicle_width_m; const double l = stop_line_margin; stop_line.emplace_back( @@ -132,7 +132,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } const auto & stop_pose = stop_point->second; - setDistance(autoware_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)) { @@ -229,7 +229,7 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( continue; // not stop vehicle } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = autoware_universe_utils::toPolygon2d(object); + const Polygon2d obj_footprint = autoware::universe_utils::toPolygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); @@ -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 = - autoware_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 = - autoware_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 */, "autoware_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(); @@ -318,7 +318,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } const auto no_stopping_area = no_stopping_area_reg_elem_.noStoppingAreas().front(); for (size_t i = closest_idx + num_ignore_nearest; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware_universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { is_in_area = true; @@ -339,10 +339,10 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( // decide end idx with extract distance size_t ego_area_end_idx = ego_area_start_idx; for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware_universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - dist_from_area_sum += autoware_universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); } if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { break; 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 eead30b11f531..ae297d96c5afd 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; - autoware_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/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp index dff8ce1da9098..a32cb4018cda9 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 @@ -30,12 +30,12 @@ namespace { using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerPosition; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerPosition; +using autoware::universe_utils::createMarkerScale; using occlusion_spot_utils::PossibleCollisionInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -214,12 +214,12 @@ MarkerArray OcclusionSpotModule::createDebugMarkerArray() return debug_marker_array; } -autoware_motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; wall.text = "occlusion_spot"; - wall.style = autoware_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/grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 03124a0b81b43..783970c701262 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -95,7 +95,7 @@ std::optional generateOcclusionPolygon( const Polygon2d & occupancy_poly, const Point2d & origin, const Point2d & min_theta_pos, const Point2d & max_theta_pos, const double ray_max_length = 100.0) { - using autoware_universe_utils::normalizeRadian; + using autoware::universe_utils::normalizeRadian; const double origin_x = origin.x(); const double origin_y = origin.y(); const double min_theta = @@ -134,7 +134,7 @@ std::optional generateOcclusionPolygon( Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, const double r = 100) { - using autoware_universe_utils::calcOffsetPose; + using autoware::universe_utils::calcOffsetPose; // generate occupancy polygon from grid origin Polygon2d poly; // create counter clockwise poly poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, 0, 0).position)); @@ -155,7 +155,7 @@ std::pair calcEdgePoint(const Polygon2d & foot_print, const Poin for (size_t i = 0; i < foot_print.outer().size(); i++) { const auto & f = foot_print.outer().at(i); PolarCoordinates polar = toPolarCoordinates(origin, f); - const double theta_norm = autoware_universe_utils::normalizeRadian(polar.theta, 0.0); + const double theta_norm = autoware::universe_utils::normalizeRadian(polar.theta, 0.0); if (theta_norm < min_theta) { min_theta = theta_norm; min_idx = i; @@ -183,7 +183,7 @@ std::optional generateOccupiedPolygon( Point transformFromMap2Grid(const TransformStamped & geom_tf_map2grid, const Point2d & p) { - Point geom_pt = autoware_universe_utils::createPoint(p.x(), p.y(), 0); + Point geom_pt = autoware::universe_utils::createPoint(p.x(), p.y(), 0); Point transformed_geom_pt; // from map coordinate to occupancy grid coordinate tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 60ef9c629d830..d716f17751dc0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -55,11 +55,11 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; namespace grid_utils { +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_universe_utils::LineString2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index f21d2bf6a743b..5e686cf648c50 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index cade9f0614dc0..911842ddd57ed 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -41,7 +41,7 @@ namespace occlusion_spot_utils Polygon2d toFootprintPolygon(const PredictedObject & object, const double scale = 1.0) { const Pose & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - Polygon2d obj_footprint = autoware_universe_utils::toPolygon2d(object); + Polygon2d obj_footprint = autoware::universe_utils::toPolygon2d(object); // upscale foot print for noise obj_footprint = upScalePolygon(obj_pose.position, obj_footprint, scale); return obj_footprint; @@ -126,7 +126,7 @@ void calcSlowDownPointsForPossibleCollision( const double dist_to_col = possible_collisions.at(collision_index).arc_lane_dist_at_collision.length; dist_along_next_path_point += - autoware_universe_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); + autoware::universe_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); // process if nearest possible collision is between current and next path point if (dist_along_path_point < dist_to_col) { for (; collision_index < possible_collisions.size(); collision_index++) { @@ -173,7 +173,8 @@ void clipPathByLength( double length_sum = 0; clipped.points.emplace_back(path.points.front()); for (int i = 1; i < static_cast(path.points.size()); i++) { - length_sum += autoware_universe_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); + length_sum += + autoware::universe_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); if (length_sum > max_length) return; clipped.points.emplace_back(path.points.at(i)); } @@ -248,7 +249,7 @@ void categorizeVehicles( lanelet::ArcCoordinates getOcclusionPoint( const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string) { - const auto poly = autoware_universe_utils::toPolygon2d(obj); + const auto poly = autoware::universe_utils::toPolygon2d(obj); std::deque arcs; for (const auto & p : poly.outer()) { lanelet::BasicPoint2d obj_p = {p.x(), p.y()}; @@ -374,7 +375,7 @@ std::vector filterVehiclesByDetectionArea( // stuck points by predicted objects for (const auto & object : objs) { // check if the footprint is in the stuck detect area - const auto obj_footprint = autoware_universe_utils::toPolygon2d(object); + const auto obj_footprint = autoware::universe_utils::toPolygon2d(object); for (const auto & p : polys) { if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); @@ -402,7 +403,7 @@ bool generatePossibleCollisionsFromGridMap( param.detection_area.min_occlusion_spot_size); if (param.is_show_occlusion) { for (const auto & op : occlusion_spot_positions) { - Point p = autoware_universe_utils::createPoint( + Point p = autoware::universe_utils::createPoint( op[0], op[1], path.points.at(0).point.pose.position.z); debug_data.occlusion_points.emplace_back(p); } 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 27e3452832b59..b55869be8c902 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 = autoware_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 = - autoware_motion_utils::findNearestSegmentIndex(path_interpolated.points, start_point); - const auto offset = autoware_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 4df653c26fcef..b83051fb6b6ec 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,12 +56,12 @@ class OcclusionSpotModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - autoware_motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: // Parameter PlannerParam param_; - autoware_universe_utils::StopWatch stop_watch_; + autoware::universe_utils::StopWatch stop_watch_; std::vector partition_lanelets_; protected: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index b0f4a48b9dbdc..6bc9423b6dc96 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -85,7 +85,7 @@ TEST(compareTime, polygon_vs_line_iterator) } } const grid_map::Matrix & grid_data = grid["layer"]; - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("processing_time"); size_t count = 0; [[maybe_unused]] double time = 0; 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 4ca0736ce8d7c..cbacbe625238e 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 @@ -130,9 +130,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchScenePlugin(*this, name); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -194,7 +194,7 @@ void BehaviorVelocityPlannerNode::processNoGroundPointCloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); if (!pc.empty()) { - autoware_universe_utils::transformPointCloud(pc, *pc_transformed, affine); + autoware::universe_utils::transformPointCloud(pc, *pc_transformed, affine); } planner_data_.no_ground_pointcloud = pc_transformed; @@ -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 = autoware_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/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp index 1db2766fdf686..fab047029e29c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp @@ -69,30 +69,30 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; // polling subscribers - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::PredictedObjects> sub_predicted_objects_{this, "~/input/dynamic_objects"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ - this, "~/input/no_ground_pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; + this, "~/input/no_ground_pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_acceleration_{this, "~/input/accel"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); @@ -143,9 +143,9 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; static constexpr int logger_throttle_interval = 3000; }; 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 e6300a2889a01..b09f00ce367dc 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 @@ -48,13 +48,13 @@ namespace autoware::behavior_velocity_planner { +using autoware::motion_utils::PlanningBehavior; +using autoware::motion_utils::VelocityFactor; 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 autoware::universe_utils::DebugPublisher; +using autoware::universe_utils::getOrDeclareParameter; using builtin_interfaces::msg::Time; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; @@ -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_; - autoware_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_; - autoware_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/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 09a9125ef0eac..77b7d25f9f46e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -31,7 +31,7 @@ namespace autoware::behavior_velocity_planner { namespace { -geometry_msgs::msg::Point convertToGeomPoint(const autoware_universe_utils::Point2d & p) +geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -46,7 +46,7 @@ namespace arc_lane_utils { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = - std::pair; // front index, point2d + std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset @@ -76,9 +76,9 @@ std::optional findCollisionSegment( } const auto & p1 = - autoware_universe_utils::getPoint(path.points.at(i)); // Point before collision point + autoware::universe_utils::getPoint(path.points.at(i)); // Point before collision point const auto & p2 = - autoware_universe_utils::getPoint(path.points.at(i + 1)); // Point after collision point + autoware::universe_utils::getPoint(path.points.at(i + 1)); // Point after collision point const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); @@ -106,7 +106,8 @@ std::optional findForwardOffsetSegment( { double sum_length = 0.0; for (size_t i = base_idx; i < path.points.size() - 1; ++i) { - sum_length += autoware_universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += + autoware::universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -125,7 +126,8 @@ std::optional findBackwardOffsetSegment( double sum_length = 0.0; const auto start = static_cast(base_idx) - 1; for (std::int32_t i = start; i >= 0; --i) { - sum_length += autoware_universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += + autoware::universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -149,13 +151,13 @@ std::optional findOffsetSegment( return findForwardOffsetSegment( path, collision_idx, offset_length + - autoware_universe_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); + autoware::universe_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); } return findBackwardOffsetSegment( path, collision_idx + 1, -offset_length + - autoware_universe_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); + autoware::universe_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } std::optional findOffsetSegment( @@ -185,8 +187,8 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse target_pose.position.x = target_point_2d.x(); target_pose.position.y = target_point_2d.y(); target_pose.position.z = interpolated_z; - const double yaw = autoware_universe_utils::calcAzimuthAngle(p_front, p_back); - target_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware::universe_utils::calcAzimuthAngle(p_front, p_back); + target_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return target_pose; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 608d136a418bb..696d4a41b7673 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -50,9 +50,9 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using Point2d = autoware_universe_utils::Point2d; -using LineString2d = autoware_universe_utils::LineString2d; -using Polygon2d = autoware_universe_utils::Polygon2d; +using Point2d = autoware::universe_utils::Point2d; +using LineString2d = autoware::universe_utils::LineString2d; +using Polygon2d = autoware::universe_utils::Polygon2d; template Point2d to_bg2d(const T & p) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index 9dd3792e2799a..591a30928fd09 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -57,9 +57,9 @@ struct TrafficSignalStamped }; using Pose = geometry_msgs::msg::Pose; -using Point2d = autoware_universe_utils::Point2d; -using LineString2d = autoware_universe_utils::LineString2d; -using Polygon2d = autoware_universe_utils::Polygon2d; +using Point2d = autoware::universe_utils::Point2d; +using LineString2d = autoware::universe_utils::LineString2d; +using Polygon2d = autoware::universe_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; using autoware_perception_msgs::msg::PredictedObjects; 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 edc67d4bbdc94..a0f30cd3e33cb 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 @@ -24,7 +24,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::StopWatch; +using autoware::universe_utils::StopWatch; SceneModuleInterface::SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) @@ -41,7 +41,7 @@ size_t SceneModuleInterface::findEgoSegmentIndex( const std::vector & points) const { const auto & p = planner_data_; - return autoware_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 autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); } @@ -236,7 +236,7 @@ UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) cons void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) { - map_uuid_.insert({module_id, autoware_universe_utils::generateUUID()}); + map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); } void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index 9061508e6d77d..dfd201bac707c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -19,11 +19,11 @@ namespace autoware::behavior_velocity_planner { namespace debug { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, 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 2581edd2c8278..62050d24e273d 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 @@ -34,7 +34,7 @@ bool splineInterpolate( return false; } - output = autoware_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, autoware_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) { @@ -79,7 +79,7 @@ autoware_planning_msgs::msg::Path interpolatePath( v.push_back(path_point.longitudinal_velocity_mps); if (idx != 0) { const auto path_point_prev = path.points.at(idx - 1); - s += autoware_universe_utils::calcDistance2d(path_point_prev.pose, path_point.pose); + s += autoware::universe_utils::calcDistance2d(path_point_prev.pose, path_point.pose); } if (s > path_len) { break; @@ -122,7 +122,7 @@ autoware_planning_msgs::msg::Path interpolatePath( return path; } - return autoware_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 00b34fbcac61f..5900bd3531266 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 @@ -55,7 +55,7 @@ bool smoothPath( const auto & smoother = planner_data->velocity_smoother_; auto trajectory = - autoware_motion_utils::convertToTrajectoryPoints( + autoware::motion_utils::convertToTrajectoryPoints( in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); @@ -67,7 +67,7 @@ bool smoothPath( 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 = - autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); std::vector debug_trajectories; @@ -87,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 = autoware_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 087e532b6e0df..457c3a1271e31 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 @@ -46,9 +46,9 @@ size_t calcPointIndexFromSegmentIndex( const size_t next_point_idx = seg_idx + 1; const double prev_dist = - autoware_universe_utils::calcDistance2d(point, points.at(prev_point_idx)); + autoware::universe_utils::calcDistance2d(point, points.at(prev_point_idx)); const double next_dist = - autoware_universe_utils::calcDistance2d(point, points.at(next_point_idx)); + autoware::universe_utils::calcDistance2d(point, points.at(next_point_idx)); if (prev_dist < next_dist) { return prev_point_idx; @@ -62,7 +62,7 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con { auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; PathPoint p; - p.pose = autoware_universe_utils::calcInterpolatedPose(p0, p1, ratio); + p.pose = autoware::universe_utils::calcInterpolatedPose(p0, p1, ratio); const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); p.longitudinal_velocity_mps = v; return p; @@ -84,7 +84,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); res.position.z = target.position.z - origin.position.z; res.orientation = - autoware_universe_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); + autoware::universe_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); return res; } @@ -95,16 +95,16 @@ 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::motion_utils::calcLongitudinalOffsetToSegment; +using autoware::motion_utils::calcSignedArcLength; +using autoware::motion_utils::validateNonEmpty; +using autoware::universe_utils::calcAzimuthAngle; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::calcSquaredDistance2d; +using autoware::universe_utils::createQuaternionFromYaw; +using autoware::universe_utils::getPoint; using autoware_planning_msgs::msg::PathPoint; -using autoware_universe_utils::calcAzimuthAngle; -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::calcSquaredDistance2d; -using autoware_universe_utils::createQuaternionFromYaw; -using autoware_universe_utils::getPoint; size_t calcSegmentIndexFromPointIndex( const std::vector & points, @@ -121,7 +121,7 @@ size_t calcSegmentIndexFromPointIndex( } const double offset_to_seg = - autoware_motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); if (0 < offset_to_seg) { return idx; } @@ -318,7 +318,7 @@ geometry_msgs::msg::Pose getAheadPose( for (size_t i = start_idx; i < path.points.size() - 1; ++i) { const geometry_msgs::msg::Pose p0 = path.points.at(i).point.pose; const geometry_msgs::msg::Pose p1 = path.points.at(i + 1).point.pose; - curr_dist += autoware_universe_utils::calcDistance2d(p0, p1); + curr_dist += autoware::universe_utils::calcDistance2d(p0, p1); if (curr_dist > ahead_dist) { const double dl = std::max(curr_dist - prev_dist, 0.0001 /* avoid 0 divide */); const double w_p0 = (curr_dist - ahead_dist) / dl; @@ -613,7 +613,7 @@ 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 autoware_motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( path.points, self_pose.position, line_pose.position) + offset < 0.0; @@ -626,8 +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 = autoware_motion_utils::findNearestSegmentIndex(output.points, stop_point); - const auto insert_idx = autoware_motion_utils::insertTargetPoint( + 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) { @@ -639,35 +640,36 @@ std::optional insertDecelPoint( output.points.at(i).point.longitudinal_velocity_mps = std::min(original_velocity, target_velocity); } - return autoware_universe_utils::getPose(output.points.at(insert_idx.value())); + return autoware::universe_utils::getPose(output.points.at(insert_idx.value())); } // TODO(murooka): remove this function for u-turn and crossing-path std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output) { - const size_t base_idx = autoware_motion_utils::findNearestSegmentIndex(output.points, stop_point); + 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); + autoware::motion_utils::insertStopPoint(base_idx, stop_point, output.points); if (!insert_idx) { return {}; } - return autoware_universe_utils::getPose(output.points.at(insert_idx.value())); + return autoware::universe_utils::getPose(output.points.at(insert_idx.value())); } std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output) { const auto insert_idx = - autoware_motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points); + autoware::motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points); if (!insert_idx) { return {}; } - return autoware_universe_utils::getPose(output.points.at(insert_idx.value())); + return autoware::universe_utils::getPose(output.points.at(insert_idx.value())); } std::set getAssociativeIntersectionLanelets( 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 a4ee3806417f7..ca5c57fd522c9 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 @@ -84,8 +84,8 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { using autoware::behavior_velocity_planner::interpolatePath; - using autoware_motion_utils::calcSignedArcLength; - using autoware_motion_utils::searchZeroVelocityIndex; + using autoware::motion_utils::calcSignedArcLength; + using autoware::motion_utils::searchZeroVelocityIndex; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; 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 bdd62fe6aa083..330a3443b4e5d 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 @@ -20,13 +20,13 @@ #include #include -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::calcOffsetPose; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerScale; -using autoware_universe_utils::createPoint; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; namespace autoware::behavior_velocity_planner { @@ -35,7 +35,7 @@ namespace RunOutDebug::TextWithPosition createDebugText( const std::string text, const geometry_msgs::msg::Pose pose, const float lateral_offset) { - const auto offset_pose = autoware_universe_utils::calcOffsetPose(pose, 0, lateral_offset, 0); + const auto offset_pose = autoware::universe_utils::calcOffsetPose(pose, 0, lateral_offset, 0); RunOutDebug::TextWithPosition text_with_position; text_with_position.text = text; @@ -136,7 +136,7 @@ void RunOutDebug::pushDetectionAreaPolygons(const Polygon2d & debug_polygon) { std::vector ros_points; for (const auto & p : debug_polygon.outer()) { - ros_points.push_back(autoware_universe_utils::createPoint(p.x(), p.y(), 0.0)); + ros_points.push_back(autoware::universe_utils::createPoint(p.x(), p.y(), 0.0)); } detection_area_polygons_.push_back(ros_points); @@ -146,7 +146,7 @@ void RunOutDebug::pushMandatoryDetectionAreaPolygons(const Polygon2d & debug_pol { std::vector ros_points; for (const auto & p : debug_polygon.outer()) { - ros_points.push_back(autoware_universe_utils::createPoint(p.x(), p.y(), 0.0)); + ros_points.push_back(autoware::universe_utils::createPoint(p.x(), p.y(), 0.0)); } mandatory_detection_area_polygons_.push_back(ros_points); @@ -186,12 +186,12 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray return visualization_marker_array; } -autoware_motion_utils::VirtualWalls RunOutDebug::createVirtualWalls() +autoware::motion_utils::VirtualWalls RunOutDebug::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; wall.text = "run_out"; - wall.style = autoware_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(); } -autoware_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 7aaa60e2e4b69..b9ea77fc91e8c 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 @@ -123,7 +123,7 @@ class RunOutDebug void publishEmptyPointCloud(); visualization_msgs::msg::MarkerArray createVisualizationMarkerArray(); void setHeight(const double height); - autoware_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 73e97a33ad8de..f75da2b5cc5de 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 @@ -38,16 +38,16 @@ namespace geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory( const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & point) { - const auto nearest_idx = autoware_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 = - autoware_universe_utils::calcLongitudinalDeviation(nearest_pose, point); + autoware::universe_utils::calcLongitudinalDeviation(nearest_pose, point); const auto vertical_point = - autoware_universe_utils::calcOffsetPose(nearest_pose, longitudinal_offset, 0, 0).position; - const auto azimuth_angle = autoware_universe_utils::calcAzimuthAngle(point, vertical_point); + autoware::universe_utils::calcOffsetPose(nearest_pose, longitudinal_offset, 0, 0).position; + const auto azimuth_angle = autoware::universe_utils::calcAzimuthAngle(point, vertical_point); - return autoware_universe_utils::createQuaternionFromYaw(azimuth_angle); + return autoware::universe_utils::createQuaternionFromYaw(azimuth_angle); } // create predicted path assuming that obstacles move with constant velocity @@ -60,7 +60,7 @@ std::vector createPredictedPath( for (size_t i = 0; i < path_size; i++) { const float travel_dist = max_velocity_mps * time_step * i; const auto predicted_pose = - autoware_universe_utils::calcOffsetPose(initial_pose, travel_dist, 0, 0); + autoware::universe_utils::calcOffsetPose(initial_pose, travel_dist, 0, 0); path_points.emplace_back(predicted_pose); } @@ -115,7 +115,7 @@ bool isAheadOf( const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose & base_pose) { const auto longitudinal_deviation = - autoware_universe_utils::calcLongitudinalDeviation(base_pose, target_point); + autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point); const bool is_ahead = longitudinal_deviation > 0; return is_ahead; } @@ -136,7 +136,7 @@ pcl::PointCloud extractObstaclePointsWithinPolygon( pcl::PointCloud output_points; output_points.header = input_points.header; for (const auto & poly : polys) { - const auto bounding_box = bg::return_envelope(poly); + const auto bounding_box = bg::return_envelope(poly); for (const auto & p : input_points) { Point2d point(p.x, p.y); @@ -165,9 +165,9 @@ std::vector> groupPointsWithNearestSegmentIndex( points_with_index.resize(path_points.size()); for (const auto & p : input_points.points) { - const auto ros_point = autoware_universe_utils::createPoint(p.x, p.y, p.z); + const auto ros_point = autoware::universe_utils::createPoint(p.x, p.y, p.z); const size_t nearest_seg_idx = - autoware_motion_utils::findNearestSegmentIndex(path_points, ros_point); + 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 ( @@ -189,10 +189,10 @@ pcl::PointXYZ calculateLateralNearestPoint( { const auto lateral_nearest_point = std::min_element( input_points.points.begin(), input_points.points.end(), [&](const auto & p1, const auto & p2) { - const auto lateral_deviation_p1 = std::abs(autoware_universe_utils::calcLateralDeviation( - base_pose, autoware_universe_utils::createPoint(p1.x, p1.y, 0))); - const auto lateral_deviation_p2 = std::abs(autoware_universe_utils::calcLateralDeviation( - base_pose, autoware_universe_utils::createPoint(p2.x, p2.y, 0))); + const auto lateral_deviation_p1 = std::abs(autoware::universe_utils::calcLateralDeviation( + base_pose, autoware::universe_utils::createPoint(p1.x, p1.y, 0))); + const auto lateral_deviation_p2 = std::abs(autoware::universe_utils::calcLateralDeviation( + base_pose, autoware::universe_utils::createPoint(p2.x, p2.y, 0))); return lateral_deviation_p1 < lateral_deviation_p2; }); @@ -267,7 +267,7 @@ pcl::PointCloud transformPointCloud( pcl::PointCloud pointcloud_pcl; pcl::fromROSMsg(input_pointcloud, pointcloud_pcl); pcl::PointCloud pointcloud_pcl_transformed; - autoware_universe_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( pointcloud_pcl, pointcloud_pcl_transformed, transform_matrix); return pointcloud_pcl_transformed; @@ -366,8 +366,8 @@ std::vector DynamicObstacleCreatorForObject::createDynamicObsta dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose; if (param_.assume_fixed_velocity) { - dynamic_obstacle.min_velocity_mps = autoware_universe_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = autoware_universe_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware::universe_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware::universe_utils::kmph2mps(param_.max_vel_kmph); } else { calculateMinAndMaxVelFromCovariance( predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier, @@ -409,8 +409,8 @@ std::vector DynamicObstacleCreatorForObjectWithoutPath::createD dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); - dynamic_obstacle.min_velocity_mps = autoware_universe_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = autoware_universe_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware::universe_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware::universe_utils::kmph2mps(param_.max_vel_kmph); dynamic_obstacle.classifications = predicted_object.classification; dynamic_obstacle.shape = predicted_object.shape; @@ -469,12 +469,12 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta // create pose facing the direction of the lane dynamic_obstacle.pose.position = - autoware_universe_utils::createPoint(point.x, point.y, point.z); + autoware::universe_utils::createPoint(point.x, point.y, point.z); dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); - dynamic_obstacle.min_velocity_mps = autoware_universe_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = autoware_universe_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware::universe_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware::universe_utils::kmph2mps(param_.max_vel_kmph); // create classification of points as pedestrian ObjectClassification classification; @@ -495,7 +495,7 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta param_.max_prediction_time); predicted_path.confidence = 1.0; dynamic_obstacle.predicted_paths.emplace_back(predicted_path); - dynamic_obstacle.uuid = autoware_universe_utils::generateUUID(); + dynamic_obstacle.uuid = autoware::universe_utils::generateUUID(); dynamic_obstacles.emplace_back(dynamic_obstacle); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 00a44a0cf625e..f2261aa8f4640 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -22,7 +22,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { 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 7a3083e75f25c..2196cdec36886 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 @@ -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 = autoware_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 11db6a604a30e..4cede13e56469 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 @@ -177,7 +177,7 @@ bool RunOutModule::modifyPathVelocity( insertStopPoint(last_stop_point_, *path); // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( *last_stop_point_, planner_param_.vehicle_param.base_to_front, 0, 0)); } } @@ -222,7 +222,7 @@ std::optional RunOutModule::detectCollision( const auto & p2 = path.points.at(idx).point; const float prev_vel = std::max(p1.longitudinal_velocity_mps, planner_param_.run_out.min_vel_ego_kmph / 3.6f); - const float ds = autoware_universe_utils::calcDistance2d(p1, p2); + const float ds = autoware::universe_utils::calcDistance2d(p1, p2); // calculate travel time from nearest point to p2 travel_time += ds / prev_vel; @@ -272,9 +272,9 @@ 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 = autoware_motion_utils::calcSignedArcLength( + const auto dist_lhs = autoware::motion_utils::calcSignedArcLength( path.points, base_pose.position, lhs.pose.position); - const auto dist_rhs = autoware_motion_utils::calcSignedArcLength( + const auto dist_rhs = autoware::motion_utils::calcSignedArcLength( path.points, base_pose.position, rhs.pose.position); return dist_lhs < dist_rhs; @@ -316,10 +316,10 @@ std::optional RunOutModule::findNearestCollisionObstacle( float RunOutModule::calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const { - const auto vehicle_front_pose = autoware_universe_utils::calcOffsetPose( + const auto vehicle_front_pose = autoware::universe_utils::calcOffsetPose( base_pose, planner_param_.vehicle_param.base_to_front, 0, 0); const auto longitudinal_offset_from_front = - std::abs(autoware_universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point)); + std::abs(autoware::universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point)); return longitudinal_offset_from_front; } @@ -341,7 +341,7 @@ std::vector RunOutModule::createVehiclePolygon( const double base_to_left = (planner_param_.vehicle_param.wheel_tread / 2.0) + planner_param_.vehicle_param.left_overhang; - using autoware_universe_utils::calcOffsetPose; + using autoware::universe_utils::calcOffsetPose; const auto p1 = calcOffsetPose(base_pose, base_to_front, base_to_left, 0.0); const auto p2 = calcOffsetPose(base_pose, base_to_front, -base_to_right, 0.0); const auto p3 = calcOffsetPose(base_pose, -base_to_rear, -base_to_right, 0.0); @@ -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 autoware::motion_utils::findNearestIndex; + using autoware::universe_utils::calcOffsetPose; if (!planner_param_.run_out.exclude_obstacles_already_in_path || path.points.empty()) { return dynamic_obstacles; } @@ -379,18 +379,18 @@ std::vector RunOutModule::excludeObstaclesOnEgoPath( } const auto object_position = o.pose.position; const auto closest_ego_pose = path.points.at(*idx).point.pose; - const auto vehicle_left_pose = autoware_universe_utils::calcOffsetPose( + const auto vehicle_left_pose = autoware::universe_utils::calcOffsetPose( closest_ego_pose, 0, vehicle_with_with_margin_halved, 0); - const auto vehicle_right_pose = autoware_universe_utils::calcOffsetPose( + const auto vehicle_right_pose = autoware::universe_utils::calcOffsetPose( closest_ego_pose, 0, -vehicle_with_with_margin_halved, 0); const double signed_distance_from_left = - autoware_universe_utils::calcLateralDeviation(vehicle_left_pose, object_position); + autoware::universe_utils::calcLateralDeviation(vehicle_left_pose, object_position); const double signed_distance_from_right = - autoware_universe_utils::calcLateralDeviation(vehicle_right_pose, object_position); + autoware::universe_utils::calcLateralDeviation(vehicle_right_pose, object_position); // If object is outside of the ego path, include it in list of possible target objects // It is also deleted from the path of objects inside ego path - const auto obstacle_uuid_hex = autoware_universe_utils::toHexString(o.uuid); + const auto obstacle_uuid_hex = autoware::universe_utils::toHexString(o.uuid); if (signed_distance_from_left > 0.0 || signed_distance_from_right < 0.0) { obstacles_outside_of_path.push_back(o); obstacle_in_ego_path_times_.erase(obstacle_uuid_hex); @@ -482,7 +482,7 @@ std::optional RunOutModule::calcPredictedObstaclePose( const auto & p1 = predicted_path.at(i - 1); const auto & p2 = predicted_path.at(i); - const float ds = autoware_universe_utils::calcDistance2d(p1, p2); + const float ds = autoware::universe_utils::calcDistance2d(p1, p2); const float dt = ds / velocity_mps; // apply linear interpolation between the predicted path points @@ -555,7 +555,7 @@ bool RunOutModule::checkCollisionWithCylinder( run_out_utils::createBoostPolyFromMsg(bounding_box_for_points); // check collision with 2d polygon - std::vector collision_points_bg; + std::vector collision_points_bg; bg::intersection(vehicle_polygon, bg_bounding_box_for_points, collision_points_bg); // no collision detected @@ -568,7 +568,7 @@ bool RunOutModule::checkCollisionWithCylinder( debug_ptr_->pushCollisionObstaclePolygons(bounding_box_for_points); for (const auto & p : collision_points_bg) { const auto p_msg = - autoware_universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + autoware::universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); collision_points.emplace_back(p_msg); } @@ -581,30 +581,30 @@ std::vector RunOutModule::createBoundingBoxForRangedP const PoseWithRange & pose_with_range, const float x_offset, const float y_offset) const { const auto dist_p1_p2 = - autoware_universe_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); + autoware::universe_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); geometry_msgs::msg::Pose p_min_to_p_max; if (dist_p1_p2 < std::numeric_limits::epsilon()) { // can't calculate the angle if two points are the same p_min_to_p_max = pose_with_range.pose_min; } else { - const auto azimuth_angle = autoware_universe_utils::calcAzimuthAngle( + const auto azimuth_angle = autoware::universe_utils::calcAzimuthAngle( pose_with_range.pose_min.position, pose_with_range.pose_max.position); p_min_to_p_max.position = pose_with_range.pose_min.position; - p_min_to_p_max.orientation = autoware_universe_utils::createQuaternionFromYaw(azimuth_angle); + p_min_to_p_max.orientation = autoware::universe_utils::createQuaternionFromYaw(azimuth_angle); } std::vector poly; poly.emplace_back( - autoware_universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, y_offset, 0.0) + autoware::universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, y_offset, 0.0) .position); poly.emplace_back( - autoware_universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, -y_offset, 0.0) + autoware::universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, -y_offset, 0.0) .position); poly.emplace_back( - autoware_universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, -y_offset, 0.0).position); + autoware::universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, -y_offset, 0.0).position); poly.emplace_back( - autoware_universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, y_offset, 0.0).position); + autoware::universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, y_offset, 0.0).position); return poly; } @@ -620,7 +620,7 @@ bool RunOutModule::checkCollisionWithBoundingBox( const auto bg_bounding_box = run_out_utils::createBoostPolyFromMsg(bounding_box); // check collision with 2d polygon - std::vector collision_points_bg; + std::vector collision_points_bg; bg::intersection(vehicle_polygon, bg_bounding_box, collision_points_bg); // no collision detected @@ -633,7 +633,7 @@ bool RunOutModule::checkCollisionWithBoundingBox( debug_ptr_->pushCollisionObstaclePolygons(bounding_box); for (const auto & p : collision_points_bg) { const auto p_msg = - autoware_universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + autoware::universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); collision_points.emplace_back(p_msg); } @@ -658,7 +658,7 @@ std::optional RunOutModule::calcStopPoint( } // calculate distance to collision with the obstacle - const float dist_to_collision_point = autoware_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 = autoware_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."); @@ -678,7 +678,7 @@ std::optional RunOutModule::calcStopPoint( // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return 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 = autoware_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 = autoware_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."); @@ -736,7 +736,7 @@ std::optional RunOutModule::calcStopPoint( // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; @@ -754,7 +754,7 @@ bool RunOutModule::insertStopPoint( // find nearest point index behind the stop point const auto nearest_seg_idx = - autoware_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 = - autoware_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 = - autoware_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 = autoware_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."); @@ -872,11 +872,11 @@ void RunOutModule::insertApproachingVelocity( } // debug - debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); const auto nearest_seg_idx_stop = - autoware_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 = - autoware_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(autoware_motion_utils::calcLateralOffset( + 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 = - autoware_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 = autoware_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 80404b69232e1..1922cba9257ab 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; - autoware_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 3745c01f51dd3..537d647883ad4 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 @@ -102,14 +102,14 @@ std::vector findLateralSameSidePoints( const geometry_msgs::msg::Point & target_point) { const auto signed_deviation = - autoware_universe_utils::calcLateralDeviation(base_pose, target_point); + autoware::universe_utils::calcLateralDeviation(base_pose, target_point); RCLCPP_DEBUG_STREAM( rclcpp::get_logger("findLateralSameSidePoints"), "signed dev of target: " << signed_deviation); std::vector same_side_points; for (const auto & p : points) { const auto signed_deviation_of_point = - autoware_universe_utils::calcLateralDeviation(base_pose, p); + autoware::universe_utils::calcLateralDeviation(base_pose, p); RCLCPP_DEBUG_STREAM( rclcpp::get_logger("findLateralSameSidePoints"), "signed dev of point: " << signed_deviation_of_point); @@ -127,7 +127,7 @@ std::vector findLateralSameSidePoints( bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) { - if (autoware_universe_utils::calcDistance2d(p1, p2) < std::numeric_limits::epsilon()) { + if (autoware::universe_utils::calcDistance2d(p1, p2) < std::numeric_limits::epsilon()) { return true; } @@ -184,15 +184,15 @@ bool pathIntersectsEgoCutLine( { if (path.size() < 2) return false; const auto p1 = - autoware_universe_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position; const auto p2 = - autoware_universe_utils::calcOffsetPose(ego_pose, 0.0, -half_line_length, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, 0.0, -half_line_length, 0.0).position; ego_cut_line = {p1, p2}; for (size_t i = 1; i < path.size(); ++i) { const auto & p3 = path.at(i).position; const auto & p4 = path.at(i - 1).position; - const auto intersection = autoware_universe_utils::intersect(p1, p2, p3, p4); + const auto intersection = autoware::universe_utils::intersect(p1, p2, p3, p4); if (intersection.has_value()) { return true; } @@ -218,7 +218,7 @@ std::vector excludeObstaclesOutSideOfLine( std::vector extracted_dynamic_obstacle; for (const auto & obstacle : dynamic_obstacles) { const auto obstacle_nearest_idx = - autoware_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; @@ -255,7 +255,7 @@ PathPointsWithLaneId decimatePathPoints( for (size_t i = 1; i < input_path_points.size(); i++) { const auto p1 = input_path_points.at(i - 1); const auto p2 = input_path_points.at(i); - const auto dist = autoware_universe_utils::calcDistance2d(p1, p2); + const auto dist = autoware::universe_utils::calcDistance2d(p1, p2); dist_sum += dist; if (dist_sum > step) { @@ -273,7 +273,7 @@ PathWithLaneId trimPathFromSelfPose( const double trim_distance) { const size_t nearest_idx = - autoware_motion_utils::findNearestIndex(input.points, self_pose.position); + autoware::motion_utils::findNearestIndex(input.points, self_pose.position); PathWithLaneId output{}; output.header = input.header; @@ -285,7 +285,7 @@ PathWithLaneId trimPathFromSelfPose( if (i != nearest_idx) { dist_sum += - autoware_universe_utils::calcDistance2d(input.points.at(i - 1), input.points.at(i)); + autoware::universe_utils::calcDistance2d(input.points.at(i - 1), input.points.at(i)); } if (dist_sum > trim_distance) { @@ -329,7 +329,7 @@ PathPointWithLaneId createExtendPathPoint( { PathPointWithLaneId extend_path_point = goal_point; extend_path_point.point.pose = - autoware_universe_utils::calcOffsetPose(goal_point.point.pose, extend_distance, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(goal_point.point.pose, extend_distance, 0.0, 0.0); return extend_path_point; } @@ -381,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 = autoware_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) { @@ -401,9 +401,10 @@ 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 = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, pd.current_odometry->pose, pd.ego_nearest_dist_threshold, - pd.ego_nearest_yaw_threshold); + 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( detection_area_poly, path, pd.current_odometry->pose, ego_seg_idx, da_range, pp.dynamic_obstacle.max_vel_kmph / 3.6); @@ -425,7 +426,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 = autoware_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) { @@ -443,9 +444,10 @@ 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 = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, pd.current_odometry->pose, pd.ego_nearest_dist_threshold, - pd.ego_nearest_yaw_threshold); + 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( detection_area_poly, path, pd.current_odometry->pose, ego_seg_idx, da_range, pp.dynamic_obstacle.max_vel_kmph / 3.6); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 0b79b7bbb9a7d..5f5b7206551ef 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -34,15 +34,15 @@ namespace autoware::behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; +using autoware::universe_utils::Box2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; -using autoware_universe_utils::Box2d; -using autoware_universe_utils::LineString2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; 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 67f149124e3ff..29295f41cdf04 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 @@ -26,9 +26,9 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; namespace { @@ -96,16 +96,16 @@ visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() return debug_marker_array; } -autoware_motion_utils::VirtualWalls StopLineModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls StopLineModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWalls virtual_walls; if (debug_data_.stop_pose && (state_ == State::APPROACH || state_ == State::STOPPED)) { - autoware_motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWall wall; wall.text = "stopline"; - wall.style = autoware_motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.ns = std::to_string(module_id_) + "_"; - wall.pose = autoware_universe_utils::calcOffsetPose( + wall.pose = autoware::universe_utils::calcOffsetPose( *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index e510e6a32f7e3..97dcb88698868 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -23,7 +23,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::TrafficSign; StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) 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 e851d0c442dc4..6075706a672d0 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 @@ -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 = autoware_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 = autoware_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 7fc9492229cc9..b6b81f224c7be 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; - autoware_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 4c90b8dd291ca..5300bb3b4b5c1 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 `autoware_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/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 768799382a27f..21c70ee41b8bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -28,7 +28,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) 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 169381b819c47..0394b0c9e381f 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 @@ -36,9 +36,9 @@ visualization_msgs::msg::MarkerArray TemplateModule::createDebugMarkerArray() return ma; }; -autoware_motion_utils::VirtualWalls TemplateModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls TemplateModule::createVirtualWalls() { - autoware_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 b3eb7f65ba265..9789ee37aa669 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,12 @@ 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 - * `autoware_motion_utils::VirtualWalls` object. + * `autoware::motion_utils::VirtualWalls` object. * - * @return A `autoware_motion_utils::VirtualWalls` object representing virtual walls in the scene. + * @return A `autoware::motion_utils::VirtualWalls` object representing virtual walls in the + * scene. */ - autoware_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/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp index 8dc7fd12a7bfc..d6d0505635079 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 @@ -32,22 +32,22 @@ visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray( return debug_marker_array; } -autoware_motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_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 = autoware_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); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } - wall.style = autoware_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); + 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/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 375533fee48f2..8f60f291131bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -26,7 +26,7 @@ #include namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::TrafficLight; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) 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 3d2b651af45f4..7770a480f8401 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 @@ -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 = autoware_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 6ac08f555fb26..387d6c1f23acf 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; - autoware_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/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 10ec9a7b961fd..1ad24cb48409d 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 @@ -18,27 +18,27 @@ #include #include #include -using autoware_motion_utils::createStopVirtualWallMarker; -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerOrientation; -using autoware_universe_utils::createMarkerPosition; -using autoware_universe_utils::createMarkerScale; -using autoware_universe_utils::toMsg; +using autoware::motion_utils::createStopVirtualWallMarker; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerPosition; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::toMsg; using namespace std::literals::string_literals; namespace autoware::behavior_velocity_planner { namespace { -[[maybe_unused]] autoware_universe_utils::LinearRing3d createCircle( - const autoware_universe_utils::Point3d & p, const double radius, const size_t num_points = 50) +[[maybe_unused]] autoware::universe_utils::LinearRing3d createCircle( + const autoware::universe_utils::Point3d & p, const double radius, const size_t num_points = 50) { - autoware_universe_utils::LinearRing3d ring; // clockwise and closed + autoware::universe_utils::LinearRing3d ring; // clockwise and closed for (size_t i = 0; i < num_points; ++i) { - const double theta = i * (2 * autoware_universe_utils::pi / num_points); + const double theta = i * (2 * autoware::universe_utils::pi / num_points); const double x = p.x() + radius * std::sin(theta); const double y = p.y() + radius * std::cos(theta); ring.emplace_back(x, y, p.z()); @@ -51,13 +51,13 @@ namespace } } // namespace -autoware_motion_utils::VirtualWalls VirtualTrafficLightModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls VirtualTrafficLightModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_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 = autoware_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/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 1009acaae768b..56f02529ddb3b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::VirtualTrafficLight; namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; @@ -49,7 +49,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); p.hold_stop_margin_distance = getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); - p.max_yaw_deviation_rad = autoware_universe_utils::deg2rad( + p.max_yaw_deviation_rad = autoware::universe_utils::deg2rad( getOrDeclareParameter(node, ns + ".max_yaw_deviation_deg")); p.check_timeout_after_stop_line = getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); @@ -59,10 +59,10 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const tier4_planning_msgs::msg::PathWithLaneId & path) { - autoware_universe_utils::LineString2d ego_path_linestring; + autoware::universe_utils::LineString2d ego_path_linestring; for (const auto & path_point : path.points) { ego_path_linestring.push_back( - autoware_universe_utils::fromMsg(path_point.point.pose.position).to_2d()); + autoware::universe_utils::fromMsg(path_point.point.pose.position).to_2d()); } for (const auto & m : planning_utils::getRegElemMapOnPath( 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 9988bbd3f6026..d27cc23223630 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 @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { namespace { -using autoware_universe_utils::calcDistance2d; +using autoware::universe_utils::calcDistance2d; struct SegmentIndexWithPoint { @@ -46,17 +46,17 @@ tier4_v2x_msgs::msg::KeyValue createKeyValue(const std::string & key, const std: return tier4_v2x_msgs::build().key(key).value(value); } -autoware_universe_utils::LineString3d toAutowarePoints( +autoware::universe_utils::LineString3d toAutowarePoints( const lanelet::ConstLineString3d & line_string) { - autoware_universe_utils::LineString3d output; + autoware::universe_utils::LineString3d output; for (const auto & p : line_string) { output.emplace_back(p.x(), p.y(), p.z()); } return output; } -std::optional toAutowarePoints( +std::optional toAutowarePoints( const lanelet::Optional & line_string) { if (!line_string) { @@ -65,28 +65,28 @@ std::optional toAutowarePoints( return toAutowarePoints(*line_string); } -std::vector toAutowarePoints( +std::vector toAutowarePoints( const lanelet::ConstLineStrings3d & line_strings) { - std::vector output; + std::vector output; for (const auto & line_string : line_strings) { output.emplace_back(toAutowarePoints(line_string)); } return output; } -[[maybe_unused]] autoware_universe_utils::LineString2d to_2d( - const autoware_universe_utils::LineString3d & line_string) +[[maybe_unused]] autoware::universe_utils::LineString2d to_2d( + const autoware::universe_utils::LineString3d & line_string) { - autoware_universe_utils::LineString2d output; + autoware::universe_utils::LineString2d output; for (const auto & p : line_string) { output.emplace_back(p.x(), p.y()); } return output; } -autoware_universe_utils::Point3d calcCenter( - const autoware_universe_utils::LineString3d & line_string) +autoware::universe_utils::Point3d calcCenter( + const autoware::universe_utils::LineString3d & line_string) { const auto p1 = line_string.front(); const auto p2 = line_string.back(); @@ -97,10 +97,10 @@ autoware_universe_utils::Point3d calcCenter( geometry_msgs::msg::Pose calcHeadPose( const geometry_msgs::msg::Pose & base_link_pose, const double base_link_to_front) { - return autoware_universe_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); + return autoware::universe_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); } -geometry_msgs::msg::Point convertToGeomPoint(const autoware_universe_utils::Point3d & p) +geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point3d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -111,7 +111,7 @@ geometry_msgs::msg::Point convertToGeomPoint(const autoware_universe_utils::Poin template std::optional findLastCollisionBeforeEndLine( - const T & points, const autoware_universe_utils::LineString3d & target_line, + const T & points, const autoware::universe_utils::LineString3d & target_line, const size_t end_line_idx) { const auto target_line_p1 = convertToGeomPoint(target_line.at(0)); @@ -119,8 +119,8 @@ std::optional findLastCollisionBeforeEndLine( for (size_t i = end_line_idx; 0 < i; --i) { // NOTE: size_t can be used since it will not be negative. - const auto & p1 = autoware_universe_utils::getPoint(points.at(i)); - const auto & p2 = autoware_universe_utils::getPoint(points.at(i - 1)); + const auto & p1 = autoware::universe_utils::getPoint(points.at(i)); + const auto & p2 = autoware::universe_utils::getPoint(points.at(i - 1)); const auto collision_point = arc_lane_utils::checkCollision(p1, p2, target_line_p1, target_line_p2); @@ -134,7 +134,7 @@ std::optional findLastCollisionBeforeEndLine( template std::optional findLastCollisionBeforeEndLine( - const T & points, const std::vector & lines, + const T & points, const std::vector & lines, const size_t end_line_idx) { for (const auto & line : lines) { @@ -158,7 +158,7 @@ std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, tier4_planning_msgs::msg::PathWithLaneId * path) { - const auto collision_offset = autoware_motion_utils::calcLongitudinalOffsetToSegment( + const auto collision_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( path->points, collision.index, collision.point); const auto offset_segment = @@ -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 = autoware_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 = autoware_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 = autoware_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 = autoware_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 = - autoware_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 = - autoware_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 e26c581ae6777..2e208f344ef4e 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 @@ -48,10 +48,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface std::string instrument_type{}; std::string instrument_id{}; std::vector custom_tags{}; - autoware_universe_utils::Point3d instrument_center{}; - std::optional stop_line{}; - autoware_universe_utils::LineString3d start_line{}; - std::vector end_lines{}; + autoware::universe_utils::Point3d instrument_center{}; + std::optional stop_line{}; + autoware::universe_utils::LineString3d start_line{}; + std::vector end_lines{}; }; struct ModuleData @@ -82,7 +82,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - autoware_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/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp index e575d6c1b7f03..59a4419087f18 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 @@ -25,14 +25,14 @@ 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 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 visualization_msgs::msg::Marker; namespace @@ -74,14 +74,14 @@ visualization_msgs::msg::MarkerArray createWalkwayMarkers( } } // namespace -autoware_motion_utils::VirtualWalls WalkwayModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls WalkwayModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_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 = autoware_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/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index 821e37ae8c618..57763f88910e2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::Crosswalk; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) 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 c876608a93df4..5a64daa0f95fc 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 @@ -22,11 +22,11 @@ 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 autoware::motion_utils::calcLongitudinalOffsetPose; +using autoware::motion_utils::calcSignedArcLength; +using autoware::motion_utils::findNearestSegmentIndex; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::getPose; 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 6ad247720b714..44ca7f0404c54 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; - autoware_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/src/debug.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp index 804d9109a7c1f..c0c04a0949ce8 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 @@ -23,11 +23,11 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::appendMarkerArray; -using autoware_universe_utils::createDefaultMarker; -using autoware_universe_utils::createMarkerColor; -using autoware_universe_utils::createMarkerScale; -using autoware_universe_utils::createPoint; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; using visualization_msgs::msg::Marker; namespace @@ -71,18 +71,18 @@ visualization_msgs::msg::MarkerArray createNoDrivableLaneMarkers( } } // namespace -autoware_motion_utils::VirtualWalls NoDrivableLaneModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls NoDrivableLaneModule::createVirtualWalls() { - autoware_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)) { - autoware_motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWall wall; wall.text = "no_drivable_lane"; - wall.style = autoware_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/manager.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 99b9dcc366eb4..9c6f2f50925c6 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -21,7 +21,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) 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 cf663d314a3c1..42d896fec8ab3 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 @@ -23,7 +23,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::createPoint; +using autoware::universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, @@ -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 = autoware_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 = autoware_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; @@ -145,17 +145,17 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR } const auto target_segment_idx = - autoware_motion_utils::findNearestSegmentIndex(path->points, target_point); + autoware::motion_utils::findNearestSegmentIndex(path->points, target_point); const auto & op_target_point_idx = - autoware_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(); } geometry_msgs::msg::Point stop_point = - autoware_universe_utils::getPoint(path->points.at(target_point_idx).point); + autoware::universe_utils::getPoint(path->points.at(target_point_idx).point); const auto & op_stop_pose = planning_utils::insertStopPoint(stop_point, target_segment_idx, *path); @@ -170,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 = autoware_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(); @@ -178,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 = - autoware_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 = - autoware_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; @@ -216,14 +216,14 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - const auto & stop_pose = autoware_universe_utils::getPose(path->points.at(0)); + const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); stop_factor.stop_pose = stop_pose; stop_factor.stop_factor_points.push_back(current_point); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); - const auto & virtual_wall_pose = autoware_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(); @@ -240,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 = autoware_motion_utils::calcLongitudinalOffsetPose( + const auto & stopped_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, planner_data_->current_odometry->pose.position, 0.0); if (!stopped_pose) { @@ -265,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 = autoware_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 d8425bdd2a292..1e8e93834f89e 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; - autoware_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 7aede9a509c79..1339fee207416 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 @@ -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 autoware::motion_utils::calcSignedArcLength; +using autoware::universe_utils::createPoint; PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLanePolygon( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, 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 fca9d3c2ec799..91f44f50e4b53 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 @@ -25,13 +25,13 @@ 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 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 visualization_msgs::msg::Marker; namespace @@ -96,15 +96,15 @@ visualization_msgs::msg::MarkerArray createSpeedBumpMarkers( } } // namespace -autoware_motion_utils::VirtualWalls SpeedBumpModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls SpeedBumpModule::createVirtualWalls() { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_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 = autoware_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); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp index 2c753f6dd2fd2..9c6546a4748b1 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { -using autoware_universe_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::SpeedBump; SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) 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 546aa0369b1f9..f0064a52891db 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 @@ -22,8 +22,8 @@ namespace autoware::behavior_velocity_planner { -using autoware_motion_utils::calcSignedArcLength; -using autoware_universe_utils::createPoint; +using autoware::motion_utils::calcSignedArcLength; +using autoware::universe_utils::createPoint; 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 504cfa243031e..75a7776d08cad 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; - autoware_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 1e00c0ac92103..66f57001b3c73 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 @@ -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 autoware::motion_utils::calcLongitudinalOffsetPoint; +using autoware::motion_utils::calcSignedArcLength; +using autoware::motion_utils::findNearestSegmentIndex; +using autoware::motion_utils::insertTargetPoint; +using autoware::universe_utils::createPoint; PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, 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 783c3f880598f..349108c138650 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 @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop std::optional find_closest_collision_point( const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose, - const autoware_universe_utils::Polygon2d & object_footprint) + const autoware::universe_utils::Polygon2d & object_footprint) { std::optional closest_collision_point; auto closest_dist = std::numeric_limits::max(); @@ -40,15 +40,15 @@ std::optional find_closest_collision_point( const auto traj_idx = rough_collision.second; const auto & ego_footprint = ego_data.trajectory_footprints[traj_idx]; const auto & ego_pose = ego_data.trajectory[traj_idx].pose; - const auto angle_diff = autoware_universe_utils::normalizeRadian( + const auto angle_diff = autoware::universe_utils::normalizeRadian( tf2::getYaw(ego_pose.orientation) - tf2::getYaw(object_pose.orientation)); if (std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { // TODO(Maxime): make this angle a parameter - autoware_universe_utils::MultiPoint2d collision_points; + autoware::universe_utils::MultiPoint2d collision_points; boost::geometry::intersection( 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 = autoware_motion_utils::calcSignedArcLength( + 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; @@ -63,7 +63,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, - const autoware_universe_utils::MultiPolygon2d & object_forward_footprints) + const autoware::universe_utils::MultiPolygon2d & object_forward_footprints) { std::vector collisions; for (auto object_idx = 0UL; object_idx < objects.size(); ++object_idx) { @@ -72,7 +72,7 @@ std::vector find_collisions( const auto collision = find_closest_collision_point(ego_data, object_pose, object_footprint); if (collision) { Collision c; - c.object_uuid = autoware_universe_utils::toHexString(objects[object_idx].object_id); + c.object_uuid = autoware::universe_utils::toHexString(objects[object_idx].object_id); c.point = *collision; collisions.push_back(c); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 78413d2faacad..00e87c69d53fa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -30,7 +30,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop /// @return the collision point closest to ego (if any) std::optional find_closest_collision_point( const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose, - const autoware_universe_utils::Polygon2d & object_footprint); + const autoware::universe_utils::Polygon2d & object_footprint); /// @brief find the earliest collision along the ego trajectory /// @param [in] ego_data ego data including its trajectory and footprint @@ -40,7 +40,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, - const autoware_universe_utils::MultiPolygon2d & obstacle_forward_footprints); + const autoware::universe_utils::MultiPolygon2d & obstacle_forward_footprints); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp index eaa148111dc49..3209a3be028ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -59,8 +59,8 @@ std::vector make_collision_markers( marker.ns = ns; marker.id = 0; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = autoware_universe_utils::createMarkerScale(0.2, 0.2, 0.5); - marker.color = autoware_universe_utils::createMarkerColor(0.6, 0.0, 0.6, 1.0); + marker.scale = autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.5); + marker.color = autoware::universe_utils::createMarkerColor(0.6, 0.0, 0.6, 1.0); for (const auto & [object_uuid, decision] : object_map) { marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker.text = object_uuid.substr(0, 5) + "\n"; @@ -85,7 +85,7 @@ std::vector make_collision_markers( } std::vector make_polygon_markers( - const autoware_universe_utils::MultiPolygon2d & footprints, const std::string & ns, + const autoware::universe_utils::MultiPolygon2d & footprints, const std::string & ns, const double z) { std::vector markers; @@ -96,8 +96,8 @@ std::vector make_polygon_markers( marker.id = 0; marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = autoware_universe_utils::createMarkerScale(0.1, 0.1, 0.1); - marker.color = autoware_universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); + marker.scale = autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1); + marker.color = autoware::universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); for (const auto & footprint : footprints) { marker.points.clear(); for (const auto & p : footprint.outer()) { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp index 35656acb0b267..2018e3e9c53a0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -39,7 +39,7 @@ std::vector make_collision_markers( const ObjectStopDecisionMap & object_map, const std::string & ns, const double z, const rclcpp::Time & now); std::vector make_polygon_markers( - const autoware_universe_utils::MultiPolygon2d & footprints, const std::string & ns, + const autoware::universe_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop::debug 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 fd8bf467ddc30..fde54a7041672 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 @@ -42,14 +42,14 @@ 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(autoware_motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - using autoware_universe_utils::getOrDeclareParameter; + using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; p.extra_object_width = getOrDeclareParameter(node, ns_ + ".extra_object_width"); p.minimum_object_velocity = getOrDeclareParameter(node, ns_ + ".minimum_object_velocity"); @@ -72,7 +72,7 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo void DynamicObstacleStopModule::update_parameters(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = params_; updateParam(parameters, ns_ + ".extra_object_width", p.extra_object_width); updateParam(parameters, ns_ + ".minimum_object_velocity", p.minimum_object_velocity); @@ -95,26 +95,26 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.reset_data(); if (ego_trajectory_points.size() < 2) return result; - autoware_universe_utils::StopWatch stopwatch; + autoware::universe_utils::StopWatch stopwatch; stopwatch.tic(); stopwatch.tic("preprocessing"); dynamic_obstacle_stop::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; ego_data.trajectory = ego_trajectory_points; - autoware_motion_utils::removeOverlapPoints(ego_data.trajectory); + autoware::motion_utils::removeOverlapPoints(ego_data.trajectory); ego_data.first_trajectory_idx = - autoware_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 = - autoware_motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( ego_data.trajectory, ego_data.first_trajectory_idx, ego_data.pose.position); - const auto min_stop_distance = autoware_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 = autoware_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 = autoware_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 - ? autoware_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; @@ -156,11 +156,11 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( result.stop_points.push_back(stop_pose->position); const auto stop_pose_reached = planner_data->current_odometry.twist.twist.linear.x < 1e-3 && - autoware_universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; + 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 ? autoware_motion_utils::VelocityFactor::STOPPED - : autoware_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) { - autoware_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 = autoware_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/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index b4f2bdc55cea1..9695146a2ad38 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -28,11 +28,11 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { -autoware_universe_utils::MultiPolygon2d make_forward_footprints( +autoware::universe_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, const PlannerParam & params, const double hysteresis) { - autoware_universe_utils::MultiPolygon2d forward_footprints; + autoware::universe_utils::MultiPolygon2d forward_footprints; for (const auto & obstacle : obstacles) forward_footprints.push_back(project_to_pose( make_forward_footprint(obstacle, params, hysteresis), @@ -40,7 +40,7 @@ autoware_universe_utils::MultiPolygon2d make_forward_footprints( return forward_footprints; } -autoware_universe_utils::Polygon2d make_forward_footprint( +autoware::universe_utils::Polygon2d make_forward_footprint( const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis) { @@ -49,7 +49,7 @@ autoware_universe_utils::Polygon2d make_forward_footprint( shape.x / 2.0 + obstacle.kinematics.initial_twist_with_covariance.twist.linear.x * params.time_horizon; const auto lateral_offset = (shape.y + params.extra_object_width) / 2.0 + hysteresis; - return autoware_universe_utils::Polygon2d{ + return autoware::universe_utils::Polygon2d{ {{longitudinal_offset, -lateral_offset}, {longitudinal_offset, lateral_offset}, {shape.x / 2.0, lateral_offset}, @@ -58,12 +58,12 @@ autoware_universe_utils::Polygon2d make_forward_footprint( {}}; } -autoware_universe_utils::Polygon2d project_to_pose( - const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +autoware::universe_utils::Polygon2d project_to_pose( + const autoware::universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); - autoware_universe_utils::Polygon2d footprint; + const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + autoware::universe_utils::Polygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.outer().emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); return footprint; @@ -72,10 +72,10 @@ autoware_universe_utils::Polygon2d project_to_pose( void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) { for (const auto & p : ego_data.trajectory) - ego_data.trajectory_footprints.push_back(autoware_universe_utils::toFootprint( + ego_data.trajectory_footprints.push_back(autoware::universe_utils::toFootprint( p.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { - const auto box = boost::geometry::return_envelope( + const auto box = boost::geometry::return_envelope( ego_data.trajectory_footprints[i]); ego_data.rtree.insert(std::make_pair(box, i)); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index 85a736869a7f8..01a112f85f4d9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop /// @param [in] params parameters used to create the footprint /// @param [in] hysteresis [m] extra lateral distance to add to the footprints /// @return forward footprint of the obstacle -autoware_universe_utils::MultiPolygon2d make_forward_footprints( +autoware::universe_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, const PlannerParam & params, const double hysteresis); /// @brief create the footprint of the given obstacle and its projection over a fixed time horizon @@ -37,15 +37,16 @@ autoware_universe_utils::MultiPolygon2d make_forward_footprints( /// @param [in] params parameters used to create the footprint /// @param [in] hysteresis [m] extra lateral distance to add to the footprint /// @return forward footprint of the obstacle -autoware_universe_utils::Polygon2d make_forward_footprint( +autoware::universe_utils::Polygon2d make_forward_footprint( const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose -autoware_universe_utils::Polygon2d project_to_pose( - const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +autoware::universe_utils::Polygon2d project_to_pose( + const autoware::universe_utils::Polygon2d & base_footprint, + const geometry_msgs::msg::Pose & pose); /// @brief create the rtree indexing the ego footprint along the trajectory /// @param [inout] ego_data ego data with its trajectory and the rtree to populate /// @param [in] params parameters 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 d227c2ae5d20c..0b78aa9707f53 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 @@ -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(autoware_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 = autoware_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 + @@ -62,7 +62,7 @@ std::vector filter_predicted_obj const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose; const auto o_yaw = tf2::getYaw(o_pose.orientation); const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation); - const auto yaw_diff = std::abs(autoware_universe_utils::normalizeRadian(o_yaw - ego_yaw)); + const auto yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(o_yaw - ego_yaw)); const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4; const auto collision_distance_threshold = params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis; 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 f60349fbf7b9f..3f544c792d8e6 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 @@ -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 = - autoware_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 = autoware_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_dynamic_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp index 0a286112010a7..294b789a679aa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -32,7 +32,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { using TrajectoryPoints = std::vector; -using BoxIndexPair = std::pair; +using BoxIndexPair = std::pair; using Rtree = boost::geometry::index::rtree>; /// @brief parameters for the "out of lane" module @@ -57,7 +57,7 @@ struct EgoData size_t first_trajectory_idx{}; double longitudinal_offset_to_first_trajectory_idx; // [m] geometry_msgs::msg::Pose pose; - autoware_universe_utils::MultiPolygon2d trajectory_footprints; + autoware::universe_utils::MultiPolygon2d trajectory_footprints; Rtree rtree; std::optional earliest_stop_pose; }; @@ -65,9 +65,9 @@ struct EgoData /// @brief debug data struct DebugData { - autoware_universe_utils::MultiPolygon2d obstacle_footprints{}; + autoware::universe_utils::MultiPolygon2d obstacle_footprints{}; size_t prev_dynamic_obstacles_nb{}; - autoware_universe_utils::MultiPolygon2d ego_footprints{}; + autoware::universe_utils::MultiPolygon2d ego_footprints{}; size_t prev_ego_footprints_nb{}; std::optional stop_pose{}; size_t prev_collisions_nb{}; 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 21dfadbe11e8a..945b94782023b 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, - autoware_motion_utils::VirtualWalls & virtual_walls) + autoware::motion_utils::VirtualWalls & virtual_walls) { std::vector slowdown_intervals; double time = 0.0; @@ -129,7 +129,7 @@ std::vector calculate_slowd if (i > 0) { const auto & prev_point = trajectory[i - 1]; time += static_cast( - autoware_universe_utils::calcDistance2d(prev_point, trajectory_point) / + autoware::universe_utils::calcDistance2d(prev_point, trajectory_point) / prev_point.longitudinal_velocity_mps); } // First linestring is used to calculate distance @@ -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))); - autoware_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 dc250af55df0b..aeeaabeb9b510 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, - autoware_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 1f621d935de38..2b78a2f1495bf 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 @@ -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(autoware_motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -132,11 +132,11 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { - autoware_universe_utils::StopWatch stopwatch; + autoware::universe_utils::StopWatch stopwatch; stopwatch.tic(); VelocityPlanningResult result; stopwatch.tic("preprocessing"); - const auto ego_idx = autoware_motion_utils::findNearestIndex( + const auto ego_idx = autoware::motion_utils::findNearestIndex( ego_trajectory_points, planner_data->current_odometry.pose.pose); if (!ego_idx) { RCLCPP_WARN_THROTTLE( @@ -183,7 +183,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( obstacle_params_); } const auto obstacles_us = stopwatch.toc("obstacles"); - autoware_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 = autoware_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_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index 430c19c3d35ff..7498568be99fb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp @@ -31,7 +31,7 @@ size_t calculateStartIndex( auto dist = 0.0; auto idx = ego_idx; while (idx + 1 < trajectory.size() && dist < start_distance) { - dist += autoware_universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); + dist += autoware::universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); ++idx; } return idx; @@ -46,7 +46,7 @@ size_t calculateEndIndex( auto idx = start_idx; while (idx + 1 < trajectory.size() && length < max_length && duration < max_duration) { const auto length_d = - autoware_universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); + autoware::universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); length += length_d; if (trajectory[idx].longitudinal_velocity_mps > 0.0) duration += length_d / trajectory[idx].longitudinal_velocity_mps; @@ -74,7 +74,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b for (auto i = 1ul; i < trajectory.size(); ++i) { const auto & prev_point = trajectory[i - 1]; auto & point = trajectory[i]; - const auto dt = autoware_universe_utils::calcDistance2d(prev_point, point) / + const auto dt = autoware::universe_utils::calcDistance2d(prev_point, point) / prev_point.longitudinal_velocity_mps; t += dt; const auto heading = tf2::getYaw(point.pose.orientation); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp index dcab3d8d1bcac..bd707402ce169 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp @@ -35,13 +35,13 @@ using nav_msgs::msg::OccupancyGrid; using PointCloud = pcl::PointCloud; using TrajectoryPoints = std::vector; -using point_t = autoware_universe_utils::Point2d; -using multipoint_t = autoware_universe_utils::MultiPoint2d; -using polygon_t = autoware_universe_utils::Polygon2d; -using multi_polygon_t = autoware_universe_utils::MultiPolygon2d; -using segment_t = autoware_universe_utils::Segment2d; -using linestring_t = autoware_universe_utils::LineString2d; -using multi_linestring_t = autoware_universe_utils::MultiLineString2d; +using point_t = autoware::universe_utils::Point2d; +using multipoint_t = autoware::universe_utils::MultiPoint2d; +using polygon_t = autoware::universe_utils::Polygon2d; +using multi_polygon_t = autoware::universe_utils::MultiPolygon2d; +using segment_t = autoware::universe_utils::Segment2d; +using linestring_t = autoware::universe_utils::LineString2d; +using multi_linestring_t = autoware::universe_utils::MultiLineString2d; struct ObstacleMasks { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index 0112de62f3950..ac07c62f62cf7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -400,7 +400,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; object1.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; object1.kinematics.initial_pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(0.0); + autoware::universe_utils::createQuaternionFromYaw(0.0); object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; @@ -427,7 +427,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_pose_with_covariance.pose.position.x = 10.0; object2.kinematics.initial_pose_with_covariance.pose.position.y = 10.0; object2.kinematics.initial_pose_with_covariance.pose.orientation = - autoware_universe_utils::createQuaternionFromYaw(M_PI_2); + autoware::universe_utils::createQuaternionFromYaw(M_PI_2); object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; 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 8ae3005c03b98..0c793c9f5798e 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 @@ -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 = autoware_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); @@ -39,18 +39,18 @@ bool can_decelerate( std::optional calculate_last_in_lane_pose( const EgoData & ego_data, const Slowdown & decision, - const autoware_universe_utils::Polygon2d & footprint, + const autoware::universe_utils::Polygon2d & footprint, const std::optional & prev_slowdown_point, const PlannerParam & params) { - const auto from_arc_length = autoware_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( + 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 = - autoware_motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + 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/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 47ea84dd74a06..ee4897de86c5b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -42,7 +42,7 @@ bool can_decelerate( /// @return the last pose that is not out of lane (if found) std::optional calculate_last_in_lane_pose( const EgoData & ego_data, const Slowdown & decision, - const autoware_universe_utils::Polygon2d & footprint, + const autoware::universe_utils::Polygon2d & footprint, const std::optional & prev_slowdown_point, const PlannerParam & params); /// @brief calculate the slowdown point to insert in the trajectory 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 3585ed9339f2c..d9a85e266f790 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 @@ -36,10 +36,10 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.id = 0; base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = autoware_universe_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = autoware_universe_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + base_marker.pose.position = autoware::universe_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); return base_marker; } void add_footprint_markers( @@ -52,7 +52,7 @@ void add_footprint_markers( debug_marker.points.clear(); for (const auto & p : f) debug_marker.points.push_back( - autoware_universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); debug_marker.points.push_back(debug_marker.points.front()); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; @@ -71,19 +71,19 @@ void add_current_overlap_marker( debug_marker.ns = "current_overlap"; debug_marker.points.clear(); for (const auto & p : current_footprint) - debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition(p.x(), p.y(), z)); + debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z)); if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); if (current_overlapped_lanelets.empty()) - debug_marker.color = autoware_universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); + debug_marker.color = autoware::universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); else - debug_marker.color = autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; for (const auto & ll : current_overlapped_lanelets) { debug_marker.points.clear(); for (const auto & p : ll.polygon3d()) debug_marker.points.push_back( - autoware_universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); + autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); debug_marker.points.push_back(debug_marker.points.front()); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; @@ -106,7 +106,7 @@ void add_lanelet_markers( // add a small z offset to draw above the lanelet map for (const auto & p : ll.polygon3d()) debug_marker.points.push_back( - autoware_universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); + autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); debug_marker.points.push_back(debug_marker.points.front()); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; @@ -123,20 +123,20 @@ void add_range_markers( { auto debug_marker = get_base_marker(); debug_marker.ns = "ranges"; - debug_marker.color = autoware_universe_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); + debug_marker.color = autoware::universe_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); for (const auto & range : ranges) { debug_marker.points.clear(); debug_marker.points.push_back( trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); - debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( + debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( range.entering_point.x(), range.entering_point.y(), z)); for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( + debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( + debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); } - debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( + debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( range.exiting_point.x(), range.exiting_point.y(), z)); debug_marker.points.push_back( trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); @@ -156,12 +156,12 @@ void add_decision_markers( debug_marker.action = debug_marker.ADD; debug_marker.id = 0; debug_marker.ns = "decisions"; - debug_marker.color = autoware_universe_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); + debug_marker.color = autoware::universe_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); debug_marker.points.clear(); for (const auto & range : ranges) { debug_marker.type = debug_marker.LINE_STRIP; if (range.debug.decision) { - debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( + debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( range.entering_point.x(), range.entering_point.y(), z)); debug_marker.points.push_back( range.debug.object->kinematics.initial_pose_with_covariance.pose.position); @@ -210,15 +210,16 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data.prev_current_overlapped_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", - autoware_universe_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), + autoware::universe_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), debug_data.prev_trajectory_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", - autoware_universe_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), + autoware::universe_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data.prev_ignored_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data.other_lanelets, "other_lanelets", - autoware_universe_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data.prev_other_lanelets); + autoware::universe_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), + debug_data.prev_other_lanelets); debug::add_range_markers( debug_marker_array, debug_data.ranges, debug_data.trajectory_points, debug_data.first_trajectory_idx, z, debug_data.prev_ranges); @@ -226,14 +227,14 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & return debug_marker_array; } -autoware_motion_utils::VirtualWalls create_virtual_walls( +autoware::motion_utils::VirtualWalls create_virtual_walls( const DebugData & debug_data, const PlannerParam & params) { - autoware_motion_utils::VirtualWalls virtual_walls; - autoware_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 = autoware_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 02f66a27079ae..ea225442443c8 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 @@ -24,7 +24,7 @@ namespace autoware::motion_velocity_planner::out_of_lane::debug { visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); -autoware_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 667bd714a848f..fc487a2626b88 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 @@ -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 autoware_motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); } @@ -80,18 +80,19 @@ 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 = autoware_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 = - autoware_motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = autoware_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( - autoware_motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = autoware_universe_utils::calcDistance2d( + 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; const auto enter_time = @@ -100,12 +101,12 @@ 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 = - autoware_motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = autoware_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( - autoware_motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = autoware_universe_utils::calcDistance2d( + 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); const auto exit_time = @@ -314,7 +315,7 @@ bool should_not_enter( for (const auto & object : inputs.objects.objects) { RCLCPP_DEBUG( logger, "\t\t[%s] going at %2.2fm/s", - autoware_universe_utils::toHexString(object.object_id).c_str(), + autoware::universe_utils::toHexString(object.object_id).c_str(), object.kinematics.initial_twist_with_covariance.twist.linear.x); if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); 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 b864d6d0eb56f..ecc10df43c792 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 @@ -50,7 +50,7 @@ void cut_predicted_path_beyond_line( auto cut_idx = stop_line_idx; double arc_length = 0; while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += autoware_universe_utils::calcDistance2d( + arc_length += autoware::universe_utils::calcDistance2d( predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); --cut_idx; } @@ -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 = autoware_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(autoware_motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto lat_offset_to_current_ego = 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/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index f411485a0750f..f5d68c4fa9bba 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -26,10 +26,10 @@ namespace autoware::motion_velocity_planner::out_of_lane { -autoware_universe_utils::Polygon2d make_base_footprint( +autoware::universe_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset) { - autoware_universe_utils::Polygon2d base_footprint; + autoware::universe_utils::Polygon2d base_footprint; const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; @@ -43,10 +43,10 @@ autoware_universe_utils::Polygon2d make_base_footprint( } lanelet::BasicPolygon2d project_to_pose( - const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) + const autoware::universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); @@ -66,14 +66,14 @@ std::vector calculate_trajectory_footprints( i < ego_data.trajectory_points.size() && length < range; ++i) { const auto & trajectory_pose = ego_data.trajectory_points[i].pose; const auto angle = tf2::getYaw(trajectory_pose.orientation); - const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back( p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); trajectory_footprints.push_back(footprint); if (i + 1 < ego_data.trajectory_points.size()) - length += autoware_universe_utils::calcDistance2d( + length += autoware::universe_utils::calcDistance2d( trajectory_pose, ego_data.trajectory_points[i + 1].pose); } return trajectory_footprints; @@ -84,7 +84,7 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( { const auto base_footprint = make_base_footprint(params, ignore_offset); const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp index bbcdd7b84dd18..ebe7ab0fa9d7f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -30,14 +30,15 @@ namespace out_of_lane /// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the /// footprint /// @return base ego footprint -autoware_universe_utils::Polygon2d make_base_footprint( +autoware::universe_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset = false); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose lanelet::BasicPolygon2d project_to_pose( - const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); + const autoware::universe_utils::Polygon2d & base_footprint, + const geometry_msgs::msg::Pose & pose); /// @brief calculate the trajectory footprints /// @details the resulting polygon follows the format used by the lanelet library: clockwise order /// and implicit closing edge diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index 60d627a4b8c5d..bfa38e544227f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -91,7 +91,7 @@ lanelet::ConstLanelets calculate_ignored_lanelets( lanelet::ConstLanelets ignored_lanelets; // ignore lanelets directly behind ego const auto behind = - autoware_universe_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); const auto behind_lanelets = lanelet::geometry::findWithin2d( route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); 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 dfcfd53575627..f6d81f6f036d0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -50,7 +50,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(autoware_motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -59,7 +59,7 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { - using autoware_universe_utils::getOrDeclareParameter; + using autoware::universe_utils::getOrDeclareParameter; auto & pp = params_; pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); @@ -109,7 +109,7 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) void OutOfLaneModule::update_parameters(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; auto & pp = params_; updateParam(parameters, ns_ + ".mode", pp.mode); updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); @@ -151,13 +151,13 @@ VelocityPlanningResult OutOfLaneModule::plan( const std::shared_ptr planner_data) { VelocityPlanningResult result; - autoware_universe_utils::StopWatch stopwatch; + autoware::universe_utils::StopWatch stopwatch; stopwatch.tic(); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; ego_data.trajectory_points = ego_trajectory_points; ego_data.first_trajectory_idx = - autoware_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 = autoware_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 = autoware_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 = autoware_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 = - autoware_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 = autoware_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 ? autoware_motion_utils::VelocityFactor::APPROACHING - : autoware_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_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 79db6b25e7ce5..535510453b8ba 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 @@ -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; - autoware_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_; - autoware_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_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 76d401a8cfe56..714b6fd948a32 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 @@ -102,9 +102,9 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & set_param_callback_ = this->add_on_set_parameters_callback( std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void MotionVelocityPlannerNode::on_load_plugin( @@ -193,7 +193,7 @@ MotionVelocityPlannerNode::process_no_ground_pointcloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); - if (!pc.empty()) autoware_universe_utils::transformPointCloud(pc, *pc_transformed, affine); + if (!pc.empty()) autoware::universe_utils::transformPointCloud(pc, *pc_transformed, affine); return *pc_transformed; } @@ -284,9 +284,9 @@ void MotionVelocityPlannerNode::insert_stop( const geometry_msgs::msg::Point & stop_point) const { const auto seg_idx = - autoware_motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); + autoware::motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); const auto insert_idx = - autoware_motion_utils::insertTargetPoint(seg_idx, stop_point, trajectory.points); + 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; @@ -300,13 +300,13 @@ void MotionVelocityPlannerNode::insert_slowdown( const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const { const auto from_seg_idx = - autoware_motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.from); - const auto from_insert_idx = autoware_motion_utils::insertTargetPoint( + 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 = - autoware_motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); + autoware::motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); const auto to_insert_idx = - autoware_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; @@ -336,7 +336,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s 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 = - autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold, planner_data.ego_nearest_yaw_threshold); std::vector debug_trajectories; @@ -389,7 +389,7 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; { std::unique_lock lk(mutex_); // for planner_manager_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 42c34a9a8c68d..7b771d0b04442 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -62,23 +62,23 @@ class MotionVelocityPlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr sub_trajectory_; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::PredictedObjects> sub_predicted_objects_{this, "~/input/dynamic_objects"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ - this, "~/input/no_ground_pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; - autoware_universe_utils::InterProcessPollingSubscriber + this, "~/input/no_ground_pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_acceleration_{this, "~/input/accel"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; rclcpp::Subscription::SharedPtr sub_lanelet_map_; @@ -138,9 +138,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware_planning_msgs::msg::Trajectory generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp index 54c12ed0ef543..2585c8a1141de 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -108,10 +108,10 @@ class ObstacleStopPlannerDebugNode explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); ~ObstacleStopPlannerDebugNode() {} bool pushPolygon( - const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); + const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); bool pushPose(const Pose & pose, const PoseType & type); 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 2a50cfd2e24e8..6d4f341de6071 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -77,13 +77,13 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::StopWatch; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; -using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::BoolStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -315,9 +315,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node return ret; } - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index bdd2a0a403fcb..c195ff0cc55f3 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -45,10 +45,10 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using std_msgs::msg::Header; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 1f01bf4417319..e9058f234e98f 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -367,7 +367,7 @@ void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( double dist_to_point = 0; // get distance from self to next nearest point dist_to_point += - autoware_motion_utils::calcSignedArcLength(trajectory, self_pose.position, size_t(1)); + 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++) { @@ -459,7 +459,7 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( object.kinematics.initial_twist_with_covariance.twist.linear.x); *velocity = - obj_vel_norm * std::cos(autoware_universe_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); + obj_vel_norm * std::cos(autoware::universe_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; } @@ -685,8 +685,8 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( double dist_to_first_point = 0.0; if (output_trajectory->size() > 1) { - dist_to_first_point = - autoware_motion_utils::calcSignedArcLength(*output_trajectory, self_pose.position, size_t(1)); + dist_to_first_point = 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 835417f33a4c0..bbb8dadbcee9d 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -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 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; namespace motion_planning { @@ -54,7 +54,7 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; for (const auto & point : polygon.outer()) { @@ -100,7 +100,7 @@ bool ObstacleStopPlannerDebugNode::pushPolygon( } bool ObstacleStopPlannerDebugNode::pushPolyhedron( - const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index db39b31241456..f3814a2a06a52 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::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::universe_utils::calcDistance2d; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::getPoint; +using autoware::universe_utils::getPose; +using autoware::universe_utils::getRPY; 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; ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_stop_planner", node_options) @@ -245,9 +245,9 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), createSubscriptionOptions(this)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -327,7 +327,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m // TODO(someone): support backward path const auto is_driving_forward = - autoware_motion_utils::isDrivingForwardWithTwist(input_msg->points); + 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( @@ -343,11 +343,11 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m Trajectory output_trajectory = *input_msg; TrajectoryPoints output_trajectory_points = - autoware_motion_utils::convertToTrajectoryPointArray(*input_msg); + autoware::motion_utils::convertToTrajectoryPointArray(*input_msg); // trim trajectory from self pose TrajectoryPoints base_trajectory = trimTrajectoryWithIndexFromSelfPose( - autoware_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 @@ -379,7 +379,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m resetExternalVelocityLimit(current_acc, current_vel); } - auto trajectory = autoware_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; @@ -1557,10 +1557,10 @@ void ObstacleStopPlannerNode::filterObstacles( // Check is it near to trajectory const double max_length = calcObstacleMaxLength(object.shape); - const size_t seg_idx = autoware_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)); + 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)); const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 3a4839aed7a36..fd8922c500318 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -32,13 +32,13 @@ namespace motion_planning { -using autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints; -using autoware_motion_utils::findFirstNearestIndexWithSoftConstraints; -using autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; +using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::getRPY; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_universe_utils::calcDistance2d; -using autoware_universe_utils::getRPY; std::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, @@ -327,40 +327,40 @@ void createOneStepPolygon( { // base step appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) .position); appendPointToPolygon( - polygon, autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) + polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) .position); appendPointToPolygon( - polygon, - autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0) + .position); } { // next step appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) .position); appendPointToPolygon( - polygon, autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) + polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) .position); appendPointToPolygon( - polygon, - autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0) + .position); } - polygon = autoware_universe_utils::isClockwise(polygon) + polygon = autoware::universe_utils::isClockwise(polygon) ? polygon - : autoware_universe_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); boost::geometry::convex_hull(polygon, hull_polygon); } @@ -552,7 +552,7 @@ double getNearestPointAndDistanceForPredictedObject( bool is_init = false; for (const auto & p : points.poses) { - double norm = autoware_universe_utils::calcDistance2d(p, base_pose); + double norm = autoware::universe_utils::calcDistance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; *nearest_collision_point = p.position; @@ -627,9 +627,9 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = autoware_universe_utils::isClockwise(object_polygon) + object_polygon = autoware::universe_utils::isClockwise(object_polygon) ? object_polygon - : autoware_universe_utils::inverseClockwise(object_polygon); + : autoware::universe_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -650,9 +650,9 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = autoware_universe_utils::isClockwise(object_polygon) + object_polygon = autoware::universe_utils::isClockwise(object_polygon) ? object_polygon - : autoware_universe_utils::inverseClockwise(object_polygon); + : autoware::universe_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -670,9 +670,9 @@ Polygon2d convertPolygonObjectToGeometryPolygon( object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); } object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = autoware_universe_utils::isClockwise(object_polygon) + object_polygon = autoware::universe_utils::isClockwise(object_polygon) ? object_polygon - : autoware_universe_utils::inverseClockwise(object_polygon); + : autoware::universe_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -692,7 +692,7 @@ std::optional getObstacleFromUuid( bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) { - const auto yaw = autoware_universe_utils::getRPY(ego_pose).z; + const auto yaw = autoware::universe_utils::getRPY(ego_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); const Eigen::Vector2d obstacle_vec( obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 0aae35126e8ab..1f05fc93138a2 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -176,7 +176,7 @@ void calculateCartesian( pose.position.x = it->x(); pose.position.y = it->y(); pose.position.z = 0.0; - pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); path.poses.push_back(pose); } path.yaws.push_back(path.yaws.back()); diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp index 9fdf7191a911d..101d135dcf8bf 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp @@ -80,7 +80,7 @@ struct TimeKeeper std::string accumulated_msg = "\n"; std::stringstream latest_stream; - autoware_universe_utils::StopWatch< + autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -89,8 +89,8 @@ struct DebugData { std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; - std::vector obstacles{}; - std::vector footprints{}; + std::vector obstacles{}; + std::vector footprints{}; }; struct TrajectoryParam @@ -103,7 +103,7 @@ struct TrajectoryParam void onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; // common updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); @@ -123,7 +123,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp index ceab5229b36d4..ac0686d177dfb 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp @@ -62,9 +62,9 @@ class PathSampler : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - autoware_universe_utils::InterProcessPollingSubscriber odom_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber odom_sub_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber objects_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; // debug publisher 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 51b28e7b3f2d6..a27dd1576c90d 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 @@ -42,8 +42,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = autoware_universe_utils::getPoint(t1); - const auto p2 = autoware_universe_utils::getPoint(t2); + const auto p1 = autoware::universe_utils::getPoint(t1); + const auto p2 = autoware::universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { 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 b3838ad8c44f5..edcb8c2b22061 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 @@ -47,12 +47,12 @@ std::optional getPointIndexAfter( } double sum_length = - -autoware_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) { 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)); + sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (offset < sum_length) { return i - 1; } @@ -64,7 +64,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < offset) { return i - 1; } @@ -77,7 +77,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware_universe_utils::getPose(point); + traj_point.pose = autoware::universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -126,7 +126,7 @@ size_t findEgoIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return autoware_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 autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -152,13 +152,13 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, autoware_universe_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - autoware_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"), @@ -166,7 +166,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = autoware_universe_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; 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 cd67a07260949..845d677007725 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -144,7 +144,7 @@ PathSampler::PathSampler(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult PathSampler::onParam( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "constraints.hard.max_curvature", params_.constraints.hard.max_curvature); updateParam(parameters, "constraints.hard.min_curvature", params_.constraints.hard.min_curvature); @@ -239,7 +239,7 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr) // 0. return if path is backward // TODO(Maxime): support backward path - if (!autoware_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,13 +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 = - autoware_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 = - autoware_motion_utils::convertToTrajectory(stopping_traj, path_ptr->header); + autoware::motion_utils::convertToTrajectory(stopping_traj, path_ptr->header); traj_pub_->publish(output_traj_msg); } @@ -318,14 +318,14 @@ void PathSampler::copyZ( to_traj.front().pose.position.z = from_traj.front().pose.position.z; if (from_traj.size() < 2 || to_traj.size() < 2) return; auto from = from_traj.begin() + 1; - auto s_from = autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_from = autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); auto s_to = 0.0; auto s_from_prev = 0.0; for (auto to = to_traj.begin() + 1; to + 1 != to_traj.end(); ++to) { - s_to += autoware_universe_utils::calcDistance2d(std::prev(to)->pose, to->pose); + s_to += autoware::universe_utils::calcDistance2d(std::prev(to)->pose, to->pose); for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { s_from_prev = s_from; - s_from += autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + s_from += autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); } const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); to->pose.position.z = std::prev(from)->pose.position.z + @@ -341,8 +341,8 @@ void PathSampler::copyVelocity( if (to_traj.empty() || from_traj.empty()) return; const auto closest_fn = [&](const auto & p1, const auto & p2) { - return autoware_universe_utils::calcDistance2d(p1.pose, ego_pose) <= - autoware_universe_utils::calcDistance2d(p2.pose, ego_pose); + return autoware::universe_utils::calcDistance2d(p1.pose, ego_pose) <= + autoware::universe_utils::calcDistance2d(p2.pose, ego_pose); }; const auto first_from = std::min_element(from_traj.begin(), from_traj.end() - 1, closest_fn); const auto first_to = std::min_element(to_traj.begin(), to_traj.end() - 1, closest_fn); @@ -352,14 +352,14 @@ void PathSampler::copyVelocity( to->longitudinal_velocity_mps = first_from->longitudinal_velocity_mps; auto from = first_from; - auto s_from = autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_from = autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); auto s_to = 0.0; auto s_from_prev = 0.0; for (; to + 1 != to_traj.end(); ++to) { - s_to += autoware_universe_utils::calcDistance2d(to->pose, std::next(to)->pose); + s_to += autoware::universe_utils::calcDistance2d(to->pose, std::next(to)->pose); for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { s_from_prev = s_from; - s_from += autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + s_from += autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); } if ( from->longitudinal_velocity_mps == 0.0 || std::prev(from)->longitudinal_velocity_mps == 0.0) { @@ -533,7 +533,7 @@ void PathSampler::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) { time_keeper_ptr_->tic(__func__); - const auto virtual_wall_marker = autoware_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/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index 4d6f2dbd1e008..06e8ab9042207 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -40,7 +40,7 @@ void prepareConstraints( constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); for (const auto & o : predicted_objects.objects) if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param - constraints.obstacle_polygons.push_back(autoware_universe_utils::toPolygon2d(o)); + constraints.obstacle_polygons.push_back(autoware::universe_utils::toPolygon2d(o)); constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented // TODO(Maxime): directly use lines instead of polygon 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 2676d223d9ffe..c8ca80b95d284 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 @@ -46,12 +46,12 @@ void compensateLastPose( const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; - const double dist = - autoware_universe_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double dist = autoware::universe_utils::calcDistance2d( + last_path_point.pose.position, last_traj_pose.position); const double norm_diff_yaw = [&]() { const double diff_yaw = tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return autoware_universe_utils::normalizeRadian(diff_yaw); + return autoware::universe_utils::normalizeRadian(diff_yaw); }(); if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { traj_points.push_back(convertToTrajectoryPoint(last_path_point)); @@ -63,10 +63,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = autoware_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 autoware_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 = autoware_motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = autoware_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 autoware_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 = autoware_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/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index de58b02a75623..29b3f5a8fc145 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -38,13 +38,13 @@ namespace autoware::sampler_common { -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::MultiPoint2d; -using autoware_universe_utils::MultiPolygon2d; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; -typedef std::pair BoxIndexPair; +typedef std::pair BoxIndexPair; typedef boost::geometry::index::rtree> Rtree; /// @brief data about constraint check results of a given path diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp index cdad454452f5c..7bd6b38490495 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -52,15 +52,15 @@ geometry_msgs::msg::Vector3 calculate_error_rpy( const geometry_msgs::msg::Vector3 & gyro_bias) { const geometry_msgs::msg::Vector3 rpy_0 = - autoware_universe_utils::getRPY(pose_list.front().pose.orientation); + autoware::universe_utils::getRPY(pose_list.front().pose.orientation); const geometry_msgs::msg::Vector3 rpy_1 = - autoware_universe_utils::getRPY(pose_list.back().pose.orientation); + autoware::universe_utils::getRPY(pose_list.back().pose.orientation); const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); geometry_msgs::msg::Vector3 error_rpy; - error_rpy.x = autoware_universe_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); - error_rpy.y = autoware_universe_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); - error_rpy.z = autoware_universe_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); + error_rpy.x = autoware::universe_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); + error_rpy.y = autoware::universe_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); + error_rpy.z = autoware::universe_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); return error_rpy; } diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 6ec5c9f64a623..278d2dfc28e59 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -59,7 +59,7 @@ GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) this->get_node_base_interface()->get_context()); this->get_node_timers_interface()->add_timer(timer_, nullptr); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // initialize diagnostics_info_ { @@ -150,10 +150,10 @@ void GyroBiasEstimator::timer_callback() // Check if the vehicle is moving straight const geometry_msgs::msg::Vector3 rpy_0 = - autoware_universe_utils::getRPY(pose_buf.front().pose.orientation); + autoware::universe_utils::getRPY(pose_buf.front().pose.orientation); const geometry_msgs::msg::Vector3 rpy_1 = - autoware_universe_utils::getRPY(pose_buf.back().pose.orientation); - const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian(rpy_1.z - rpy_0.z)); + autoware::universe_utils::getRPY(pose_buf.back().pose.orientation); + const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(rpy_1.z - rpy_0.z)); const double time_diff = (t1_rclcpp_time - t0_rclcpp_time).seconds(); const double yaw_vel = yaw_diff / time_diff; const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 7f8d5e9084ce6..1313f2cb06507 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -76,7 +76,7 @@ class GyroBiasEstimator : public rclcpp::Node std::optional gyro_bias_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string imu_frame_; diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index cc4efe1ba2f22..f96bab16443fe 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -25,7 +25,7 @@ std::array transformCovariance(const std::array & cov) { - using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = 0.0; max_cov = std::max(max_cov, cov[COV_IDX::X_X]); @@ -57,7 +57,7 @@ ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) : rclcpp::Node("imu_corrector", options), output_frame_(declare_parameter("base_link", "base_link")) { - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); angular_velocity_offset_x_imu_link_ = declare_parameter("angular_velocity_offset_x", 0.0); angular_velocity_offset_y_imu_link_ = declare_parameter("angular_velocity_offset_y", 0.0); diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp index 45f7b4e6142b2..0dacab4651477 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -31,7 +31,7 @@ namespace imu_corrector { class ImuCorrector : public rclcpp::Node { - using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; public: explicit ImuCorrector(const rclcpp::NodeOptions & options); @@ -53,7 +53,7 @@ class ImuCorrector : public rclcpp::Node double accel_stddev_imu_link_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string output_frame_; }; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 22fbbd09bd8fc..86c4ed5943540 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -185,8 +185,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node const std::string & original_topic_name, const std::string & postfix); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index bd67759d3d7a6..ead66d98b8be7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -172,8 +172,8 @@ class PointCloudConcatenationComponent : public rclcpp::Node void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 4b018f22aa60a..80abba9b5d1a5 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -69,8 +69,8 @@ class DistortionCorrectorComponent : public rclcpp::Node rclcpp::Subscription::SharedPtr twist_sub_; rclcpp::Publisher::SharedPtr undistorted_points_pub_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; tf2_ros::Buffer tf2_buffer_{get_clock()}; tf2_ros::TransformListener tf2_listener_{tf2_buffer_}; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index 23deceaf29c21..ce003f58dcc49 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -177,9 +177,9 @@ class Filter : public rclcpp::Node std::mutex mutex_; /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index c89c97269fd2e..4153cee7695f8 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -182,8 +182,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node const std::string & original_topic_name, const std::string & postfix); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index b3ab34da2726f..7b27d35509a98 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -40,9 +40,9 @@ #include #include -using autoware_universe_utils::LinearRing2d; -using autoware_universe_utils::MultiPoint2d; -using autoware_universe_utils::Point2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::Point2d; namespace pointcloud_preprocessor { diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index cd34c63b50de7..d376a47ae8ab0 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -26,7 +26,7 @@ #include -using autoware_universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPoint2d; namespace pointcloud_preprocessor { diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index bd96ef9d215bd..e998a79b27f77 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -75,8 +75,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index d442a91a91c5a..3586b7c999776 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -37,8 +37,8 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "concatenate_pointclouds_debug"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 79632b7a4111c..620e9e42a6864 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -62,8 +62,8 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 7b9ed5f30a3d8..e7aa1f24409e6 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -28,8 +28,8 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distortion_corrector"); stop_watch_ptr_->tic("cyclic_time"); @@ -290,11 +290,11 @@ bool DistortionCorrectorComponent::undistortPointCloud( theta += w * time_offset; baselink_quat.setValue( - 0, 0, autoware_universe_utils::sin(theta * 0.5f), - autoware_universe_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + 0, 0, autoware::universe_utils::sin(theta * 0.5f), + autoware::universe_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); const float dis = v * time_offset; - x += dis * autoware_universe_utils::cos(theta); - y += dis * autoware_universe_utils::sin(theta); + x += dis * autoware::universe_utils::cos(theta); + y += dis * autoware::universe_utils::sin(theta); baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); baselink_tf_odom.setRotation(baselink_quat); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp index 2114810b8934c..83c9058fd5fa5 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp @@ -57,8 +57,8 @@ PickupBasedVoxelGridDownsampleFilterComponent::PickupBasedVoxelGridDownsampleFil { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index d289aa9c95ea1..c9bb6dfc0fc66 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -103,7 +103,7 @@ pointcloud_preprocessor::Filter::Filter( std::bind(&Filter::filterParamCallback, this, std::placeholders::_1)); published_time_publisher_ = - std::make_unique(this); + std::make_unique(this); RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); } diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 975c04c9089d6..e05f44240b53a 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -29,8 +29,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); outlier_pointcloud_publisher_ = diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 42a1ef68d0f68..099e5757ffc0a 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -46,8 +46,8 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( { // initialize debug tool { - using autoware_universe_utils::DebugPublisher; - using autoware_universe_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "time_synchronizer"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index d35aca8decc28..def7652848642 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -16,7 +16,7 @@ namespace { -autoware_universe_utils::Box2d calcBoundingBox( +autoware::universe_utils::Box2d calcBoundingBox( const pcl::PointCloud::ConstPtr & input_cloud) { MultiPoint2d candidate_points; @@ -24,11 +24,11 @@ autoware_universe_utils::Box2d calcBoundingBox( candidate_points.emplace_back(p.x, p.y); } - return boost::geometry::return_envelope(candidate_points); + return boost::geometry::return_envelope(candidate_points); } lanelet::ConstPolygons3d calcIntersectedPolygons( - const autoware_universe_utils::Box2d & bounding_box, const lanelet::ConstPolygons3d & polygons) + const autoware::universe_utils::Box2d & bounding_box, const lanelet::ConstPolygons3d & polygons) { lanelet::ConstPolygons3d intersected_polygons; for (const auto & polygon : polygons) { diff --git a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp index ee9acbddf93b1..72a78f35251f7 100644 --- a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp +++ b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp @@ -51,7 +51,7 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node // Subscriber message_filters::Subscriber sub_radar_{}; message_filters::Subscriber sub_odometry_{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; using SyncPolicy = message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp index e148ed275af30..985f30c8e34b5 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -96,7 +96,7 @@ RadarStaticPointcloudFilterNode::RadarStaticPointcloudFilterNode( node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd"); // Subscriber - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_radar_.subscribe(this, "~/input/radar", rclcpp::QoS{1}.get_rmw_qos_profile()); sub_odometry_.subscribe(this, "~/input/odometry", rclcpp::QoS{1}.get_rmw_qos_profile()); diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 184bef8c96820..b479d7c36bb42 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -93,7 +93,7 @@ ObjectInfo::ObjectInfo( } const auto current_pose = - autoware_universe_utils::calcOffsetPose(initial_pose, move_distance, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(initial_pose, move_distance, 0.0, 0.0); // calculate tf from map to moved_object geometry_msgs::msg::Transform ros_map2moved_object; 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 a6c5a1eae4f7f..71c89fcf4df42 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 @@ -59,7 +59,7 @@ nav_msgs::msg::Odometry to_odometry( nav_msgs::msg::Odometry odometry; odometry.pose.pose.position.x = vehicle_model_ptr->getX(); odometry.pose.pose.position.y = vehicle_model_ptr->getY(); - odometry.pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY( + odometry.pose.pose.orientation = autoware::universe_utils::createQuaternionFromRPY( 0.0, ego_pitch_angle, vehicle_model_ptr->getYaw()); odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); @@ -304,8 +304,8 @@ rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( result.reason = "success"; try { - autoware_universe_utils::updateParam(parameters, "x_stddev", x_stddev_); - autoware_universe_utils::updateParam(parameters, "y_stddev", y_stddev_); + autoware::universe_utils::updateParam(parameters, "x_stddev", x_stddev_); + autoware::universe_utils::updateParam(parameters, "y_stddev", y_stddev_); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -323,7 +323,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const geometry_msgs::msg::Pose ego_pose; ego_pose.position.x = ego_x; ego_pose.position.y = ego_y; - ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); // calculate prev/next point of lanelet centerline nearest to ego pose. lanelet::Lanelet ego_lanelet; @@ -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 = - autoware_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); @@ -398,7 +398,7 @@ void SimplePlanningSimulator::on_timer() // add estimate covariance { - using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; current_odometry_.pose.covariance[COV_IDX::X_X] = x_stddev_; current_odometry_.pose.covariance[COV_IDX::Y_Y] = y_stddev_; } @@ -553,7 +553,7 @@ void SimplePlanningSimulator::add_measurement_noise( odom.twist.twist.linear.x += velocity_noise; double yaw = tf2::getYaw(odom.pose.pose.orientation); yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); - odom.pose.pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); + odom.pose.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); vel.longitudinal_velocity += static_cast(velocity_noise); @@ -686,7 +686,7 @@ void SimplePlanningSimulator::publish_acceleration() msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); msg.accel.accel.linear.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); - using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; msg.accel.covariance.at(COV_IDX::X_X) = COV; // linear x msg.accel.covariance.at(COV_IDX::Y_Y) = COV; // linear y @@ -699,7 +699,7 @@ void SimplePlanningSimulator::publish_acceleration() void SimplePlanningSimulator::publish_imu() { - using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; sensor_msgs::msg::Imu imu; imu.header.frame_id = "base_link"; diff --git a/system/default_ad_api/src/motion.hpp b/system/default_ad_api/src/motion.hpp index 25949adbc39df..053e9b2427a10 100644 --- a/system/default_ad_api/src/motion.hpp +++ b/system/default_ad_api/src/motion.hpp @@ -34,7 +34,7 @@ class MotionNode : public rclcpp::Node explicit MotionNode(const rclcpp::NodeOptions & options); private: - autoware_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 6fd27c4963f66..374316e19e6b1 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -137,7 +137,7 @@ void PlanningNode::on_timer() const auto & stop_point = factor.pose.position; const auto & points = trajectory_->points; factor.distance = - autoware_motion_utils::calcSignedArcLength(points, curr_point, stop_point); + 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 7e754efc4855f..929eb517afa2e 100644 --- a/system/default_ad_api/src/planning.hpp +++ b/system/default_ad_api/src/planning.hpp @@ -48,7 +48,7 @@ class PlanningNode : public rclcpp::Node std::vector steering_factors_; rclcpp::TimerBase::SharedPtr timer_; - using VehicleStopChecker = autoware_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/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index c20bcda5a6b70..e46c6e4341284 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -66,8 +66,8 @@ rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::paramCallback( auto prev_status_str = status_str; auto is_active = true; try { - autoware_universe_utils::updateParam(parameters, status_prefix_str, status_str); - autoware_universe_utils::updateParam(parameters, is_active_prefix_str, is_active); + autoware::universe_utils::updateParam(parameters, status_prefix_str, status_str); + autoware::universe_utils::updateParam(parameters, is_active_prefix_str, is_active); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 798da2374b62a..b77797b2a4205 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -63,14 +63,14 @@ class EmergencyHandler : public rclcpp::Node sub_hazard_status_stamped_; rclcpp::Subscription::SharedPtr sub_prev_control_command_; // Subscribers without callback - autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_vehicle_msgs::msg::ControlModeReport> sub_control_mode_{this, "~/input/control_mode"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index e6bac26677be5..5c1ca5e04de0e 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -59,7 +59,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n rcl_interfaces::msg::SetParametersResult MrmEmergencyStopOperator::onParameter( const std::vector & parameters) { - using autoware_universe_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "target_acceleration", params_.target_acceleration); updateParam(parameters, "target_jerk", params_.target_jerk); diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index feb01bfa22d3f..c908ca1a62ee2 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -72,18 +72,18 @@ class MrmHandler : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; // Subscribers without callback - autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_vehicle_msgs::msg::ControlModeReport> sub_control_mode_{this, "~/input/control_mode"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_mrm_pull_over_status_{this, "~/input/mrm/pull_over/status"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; - autoware_universe_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_adapi_v1_msgs::msg::OperationModeState> sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 8795d12718325..dbb1db027b149 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -147,7 +147,7 @@ class AutowareErrorMonitor : public rclcpp::Node void loggingErrors(const autoware_system_msgs::msg::HazardStatus & diag_array); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; #endif // SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index d00426137221f..1daa4624ec934 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -265,7 +265,7 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&AutowareErrorMonitor::onTimer, this)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void AutowareErrorMonitor::loadRequiredModules(const std::string & key) diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 4cacfb5f9770f..4e697fa18ec50 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -126,7 +126,7 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) void NTPMonitor::onTimer() { // Start to measure elapsed time - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("execution_time"); std::string error_str; diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 55636c7893d5d..2ab3e053225eb 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -517,7 +517,7 @@ void ProcessMonitor::onTimer() bool is_top_error = false; // Start to measure elapsed time - autoware_universe_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("execution_time"); int out_fd[2]; diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 79e64ba514bfe..e890a5208bf22 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -205,7 +205,7 @@ void ReactionAnalyzerNode::spawn_obstacle(const geometry_msgs::msg::Point & ego_ } } else { if ( - autoware_universe_utils::calcDistance3d(ego_pose, entity_pose_.position) < + autoware::universe_utils::calcDistance3d(ego_pose, entity_pose_.position) < node_params_.spawn_distance_threshold) { if (!spawn_object_cmd_) { spawn_object_cmd_ = true; @@ -394,7 +394,7 @@ bool ReactionAnalyzerNode::check_ego_init_correctly( } constexpr double deviation_threshold = 0.1; - const auto deviation = autoware_universe_utils::calcPoseDeviation( + const auto deviation = autoware::universe_utils::calcPoseDeviation( ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); const bool is_position_initialized_correctly = deviation.longitudinal < deviation_threshold && deviation.lateral < deviation_threshold && diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index b58c80c67f67a..02d7a3872953a 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 = autoware_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 = - autoware_motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position); + autoware::motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position); - const auto zero_vel_idx = autoware_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 = autoware_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 = - autoware_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/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index a07743417fa80..3ecadb96ee34d 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -136,8 +136,8 @@ visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & pa tf2::Quaternion quaternion; quaternion.setRPY( - autoware_universe_utils::deg2rad(params.roll), autoware_universe_utils::deg2rad(params.pitch), - autoware_universe_utils::deg2rad(params.yaw)); + autoware::universe_utils::deg2rad(params.roll), autoware::universe_utils::deg2rad(params.pitch), + autoware::universe_utils::deg2rad(params.yaw)); marker.pose.orientation = tf2::toMsg(quaternion); marker.scale.x = 0.1; // Line width @@ -224,7 +224,7 @@ size_t get_index_after_distance( size_t target_id = curr_id; double current_distance = 0.0; for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { - current_distance = autoware_universe_utils::calcDistance3d(traj.points.at(traj_id), curr_p); + current_distance = autoware::universe_utils::calcDistance3d(traj.points.at(traj_id), curr_p); if (current_distance >= distance) { break; } @@ -286,9 +286,9 @@ geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params) tf2::Quaternion entity_q_orientation; entity_q_orientation.setRPY( - autoware_universe_utils::deg2rad(entity_params.roll), - autoware_universe_utils::deg2rad(entity_params.pitch), - autoware_universe_utils::deg2rad(entity_params.yaw)); + autoware::universe_utils::deg2rad(entity_params.roll), + autoware::universe_utils::deg2rad(entity_params.pitch), + autoware::universe_utils::deg2rad(entity_params.yaw)); entity_pose.orientation = tf2::toMsg(entity_q_orientation); return entity_pose; } @@ -302,9 +302,9 @@ geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params) tf2::Quaternion pose_q_orientation; pose_q_orientation.setRPY( - autoware_universe_utils::deg2rad(pose_params.roll), - autoware_universe_utils::deg2rad(pose_params.pitch), - autoware_universe_utils::deg2rad(pose_params.yaw)); + autoware::universe_utils::deg2rad(pose_params.roll), + autoware::universe_utils::deg2rad(pose_params.pitch), + autoware::universe_utils::deg2rad(pose_params.yaw)); pose.orientation = tf2::toMsg(pose_q_orientation); return pose; } @@ -316,9 +316,9 @@ PointCloud2::SharedPtr create_entity_pointcloud_ptr( tf2::Quaternion entity_q_orientation; entity_q_orientation.setRPY( - autoware_universe_utils::deg2rad(entity_params.roll), - autoware_universe_utils::deg2rad(entity_params.pitch), - autoware_universe_utils::deg2rad(entity_params.yaw)); + autoware::universe_utils::deg2rad(entity_params.roll), + autoware::universe_utils::deg2rad(entity_params.pitch), + autoware::universe_utils::deg2rad(entity_params.yaw)); tf2::Transform tf(entity_q_orientation); const auto origin = tf2::Vector3(entity_params.x, entity_params.y, entity_params.z); tf.setOrigin(origin); @@ -395,7 +395,7 @@ bool search_pointcloud_near_pose( return std::any_of( pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), [pose, search_radius](const auto & point) { - return autoware_universe_utils::calcDistance3d(pose.position, point) <= search_radius; + return autoware::universe_utils::calcDistance3d(pose.position, point) <= search_radius; }); } @@ -406,7 +406,7 @@ bool search_predicted_objects_near_pose( return std::any_of( predicted_objects.objects.begin(), predicted_objects.objects.end(), [pose, search_radius](const PredictedObject & object) { - return autoware_universe_utils::calcDistance3d( + return autoware::universe_utils::calcDistance3d( pose.position, object.kinematics.initial_pose_with_covariance.pose.position) <= search_radius; }); @@ -420,7 +420,7 @@ bool search_detected_objects_near_pose( return std::any_of( detected_objects.objects.begin(), detected_objects.objects.end(), [pose, search_radius](const DetectedObject & object) { - return autoware_universe_utils::calcDistance3d( + return autoware::universe_utils::calcDistance3d( pose.position, object.kinematics.pose_with_covariance.pose.position) <= search_radius; }); @@ -433,7 +433,7 @@ bool search_tracked_objects_near_pose( return std::any_of( tracked_objects.objects.begin(), tracked_objects.objects.end(), [pose, search_radius](const TrackedObject & object) { - return autoware_universe_utils::calcDistance3d( + return autoware::universe_utils::calcDistance3d( pose.position, object.kinematics.pose_with_covariance.pose.position) <= search_radius; }); diff --git a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 6446511db4c29..39d81c26961bb 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -88,7 +88,7 @@ using DataStampedPtr = std::shared_ptr; class AccelBrakeMapCalibrator : public rclcpp::Node { private: - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string csv_default_map_dir_; rclcpp::Publisher::SharedPtr original_map_occ_pub_; rclcpp::Publisher::SharedPtr update_map_occ_pub_; @@ -107,13 +107,13 @@ class AccelBrakeMapCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr map_error_ratio_pub_; rclcpp::Publisher::SharedPtr calibration_status_pub_; - autoware_universe_utils::InterProcessPollingSubscriber steer_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber steer_sub_{ this, "~/input/steer"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber actuation_status_sub_{this, "~/input/actuation_status"}; - autoware_universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber actuation_cmd_sub_{this, "~/input/actuation_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber velocity_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber velocity_sub_{ this, "~/input/velocity"}; // Service @@ -374,7 +374,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node COMMAND = 1, }; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; public: explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 15d7473d9f00a..6a88d5c61a2d8 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -32,7 +32,7 @@ namespace autoware::accel_brake_map_calibrator AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options) : Node("accel_brake_map_calibrator", node_options) { - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // get parameter update_hz_ = declare_parameter("update_hz", 10.0); covariance_ = declare_parameter("initial_covariance", 0.05); @@ -217,7 +217,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod init_timer(1.0 / update_hz_); init_output_csv_timer(30.0); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void AccelBrakeMapCalibrator::init_output_csv_timer(double period_s) diff --git a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp index 2f1c0ac74b9a5..c219c162d6b37 100644 --- a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp +++ b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp @@ -60,11 +60,11 @@ class ExternalCmdConverterNode : public rclcpp::Node emergency_stop_heartbeat_sub_; // Polling Subscriber - autoware_universe_utils::InterProcessPollingSubscriber velocity_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber velocity_sub_{ this, "in/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber shift_cmd_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber shift_cmd_sub_{ this, "in/shift_cmd"}; - autoware_universe_utils::InterProcessPollingSubscriber gate_mode_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber gate_mode_sub_{ this, "in/current_gate_mode"}; void on_external_cmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr); diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp index b807e74fc27cf..181b7926337dc 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp @@ -78,9 +78,9 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief subscriber for vehicle command rclcpp::Subscription::SharedPtr sub_control_cmd_; // polling subscribers - autoware_universe_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_steering_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_steering_{ this, "~/input/steering"}; rclcpp::TimerBase::SharedPtr timer_; @@ -117,7 +117,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; DebugValues debug_steer_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index ce783cfcd32e4..c4d7ea11bbcf3 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -81,7 +81,7 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() diff --git a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp index 0fcc5fcf28863..e951f11e49ab7 100644 --- a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp @@ -47,8 +47,8 @@ struct VehicleInfo double min_height_offset_m; double max_height_offset_m; - autoware_universe_utils::LinearRing2d createFootprint(const double margin = 0.0) const; - autoware_universe_utils::LinearRing2d createFootprint( + autoware::universe_utils::LinearRing2d createFootprint(const double margin = 0.0) const; + autoware::universe_utils::LinearRing2d createFootprint( const double lat_margin, const double lon_margin) const; }; diff --git a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp index 665ee61fa67ff..db223663f1145 100644 --- a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -16,16 +16,16 @@ namespace autoware::vehicle_info_utils { -autoware_universe_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const +autoware::universe_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const { return createFootprint(margin, margin); } -autoware_universe_utils::LinearRing2d VehicleInfo::createFootprint( +autoware::universe_utils::LinearRing2d VehicleInfo::createFootprint( const double lat_margin, const double lon_margin) const { - using autoware_universe_utils::LinearRing2d; - using autoware_universe_utils::Point2d; + using autoware::universe_utils::LinearRing2d; + using autoware::universe_utils::Point2d; const double x_front = front_overhang_m + wheel_base_m + lon_margin; const double x_center = wheel_base_m / 2.0;