Skip to content

Commit

Permalink
refactor(universe_utils): add autoware namespace
Browse files Browse the repository at this point in the history
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
kosuke55 committed Jun 19, 2024
1 parent 87156a0 commit 454456c
Show file tree
Hide file tree
Showing 567 changed files with 2,944 additions and 2,924 deletions.
2 changes: 1 addition & 1 deletion common/autoware_grid_map_utils/test/benchmark.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::chrono::milliseconds> stopwatch;
autoware::universe_utils::StopWatch<std::chrono::milliseconds> stopwatch;

constexpr auto nb_iterations = 10;
constexpr auto polygon_side_vertices =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,9 @@ template <class T>
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;
}
Expand All @@ -67,27 +67,27 @@ bool validate_arguments(const T & input_points, const std::vector<double> & 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;
}

Expand All @@ -100,7 +100,7 @@ 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;
}

Expand All @@ -109,14 +109,14 @@ bool validate_arguments(const T & input_points, const double resampling_interval
RCLCPP_DEBUG(
get_logger(), "invalid argument: resampling interval is less than %f",
autoware_motion_utils::overlap_threshold);
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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -73,22 +73,22 @@ 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
Expand Down
Loading

0 comments on commit 454456c

Please sign in to comment.