Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: remove dependency on autoware_universe_utils from autoware_interpolation and autoware_motion_utils #10092

2 changes: 1 addition & 1 deletion build_depends.repos
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ repositories:
core/autoware_utils:
type: git
url: https://github.com/autowarefoundation/autoware_utils.git
version: 1.0.0
version: 1.1.0
core/autoware_lanelet2_extension:
type: git
url: https://github.com/autowarefoundation/autoware_lanelet2_extension.git
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_

#include "autoware/interpolation/interpolation_utils.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware_utils/geometry/geometry.hpp"

#include <Eigen/Core>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class SplineInterpolationPoints2d
{
std::vector<geometry_msgs::msg::Point> points_inner;
for (const auto & p : points) {
points_inner.push_back(autoware::universe_utils::getPoint(p));
points_inner.push_back(autoware_utils::get_point(p));
}
calcSplineCoefficientsInner(points_inner);
}
Expand Down
2 changes: 1 addition & 1 deletion common/autoware_interpolation/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_universe_utils</depend>
<depend>autoware_utils</depend>
<depend>eigen</depend>

<test_depend>ament_cmake_ros</test_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -89,8 +89,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));
pose.orientation = autoware_utils::create_quaternion_from_yaw(getSplineInterpolatedYaw(idx, s));
return pose;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
// limitations under the License.

#include "autoware/interpolation/spline_interpolation.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware_utils/geometry/geometry.hpp"

#include <gtest/gtest.h>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

#include "autoware/interpolation/spline_interpolation.hpp"
#include "autoware/interpolation/spline_interpolation_points_2d.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware_utils/geometry/geometry.hpp"

#include <gtest/gtest.h>

Expand All @@ -27,15 +27,15 @@ using autoware::interpolation::SplineInterpolationPoints2d;

TEST(spline_interpolation, splineYawFromPoints)
{
using autoware::universe_utils::createPoint;
using autoware_utils::create_point;

{ // straight
std::vector<geometry_msgs::msg::Point> points;
points.push_back(createPoint(0.0, 0.0, 0.0));
points.push_back(createPoint(1.0, 1.5, 0.0));
points.push_back(createPoint(2.0, 3.0, 0.0));
points.push_back(createPoint(3.0, 4.5, 0.0));
points.push_back(createPoint(4.0, 6.0, 0.0));
points.push_back(create_point(0.0, 0.0, 0.0));
points.push_back(create_point(1.0, 1.5, 0.0));
points.push_back(create_point(2.0, 3.0, 0.0));
points.push_back(create_point(3.0, 4.5, 0.0));
points.push_back(create_point(4.0, 6.0, 0.0));

const std::vector<double> ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937};

Expand All @@ -47,11 +47,11 @@ TEST(spline_interpolation, splineYawFromPoints)

{ // curve
std::vector<geometry_msgs::msg::Point> points;
points.push_back(createPoint(-2.0, -10.0, 0.0));
points.push_back(createPoint(2.0, 1.5, 0.0));
points.push_back(createPoint(3.0, 3.0, 0.0));
points.push_back(createPoint(5.0, 10.0, 0.0));
points.push_back(createPoint(10.0, 12.5, 0.0));
points.push_back(create_point(-2.0, -10.0, 0.0));
points.push_back(create_point(2.0, 1.5, 0.0));
points.push_back(create_point(3.0, 3.0, 0.0));
points.push_back(create_point(5.0, 10.0, 0.0));
points.push_back(create_point(10.0, 12.5, 0.0));

const std::vector<double> ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594};
const auto yaws = autoware::interpolation::splineYawFromPoints(points);
Expand All @@ -62,15 +62,15 @@ TEST(spline_interpolation, splineYawFromPoints)

{ // size of base_keys is 1 (infeasible to interpolate)
std::vector<geometry_msgs::msg::Point> points;
points.push_back(createPoint(1.0, 0.0, 0.0));
points.push_back(create_point(1.0, 0.0, 0.0));

EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error);
}

{ // straight: size of base_keys is 2 (edge case in the implementation)
std::vector<geometry_msgs::msg::Point> points;
points.push_back(createPoint(1.0, 0.0, 0.0));
points.push_back(createPoint(2.0, 1.5, 0.0));
points.push_back(create_point(1.0, 0.0, 0.0));
points.push_back(create_point(2.0, 1.5, 0.0));

const std::vector<double> ans{0.9827937, 0.9827937};

Expand All @@ -82,9 +82,9 @@ TEST(spline_interpolation, splineYawFromPoints)

{ // straight: size of base_keys is 3 (edge case in the implementation)
std::vector<geometry_msgs::msg::Point> points;
points.push_back(createPoint(1.0, 0.0, 0.0));
points.push_back(createPoint(2.0, 1.5, 0.0));
points.push_back(createPoint(3.0, 3.0, 0.0));
points.push_back(create_point(1.0, 0.0, 0.0));
points.push_back(create_point(2.0, 1.5, 0.0));
points.push_back(create_point(3.0, 3.0, 0.0));

const std::vector<double> ans{0.9827937, 0.9827937, 0.9827937};

Expand All @@ -97,15 +97,15 @@ TEST(spline_interpolation, splineYawFromPoints)

TEST(spline_interpolation, SplineInterpolationPoints2d)
{
using autoware::universe_utils::createPoint;
using autoware_utils::create_point;

// curve
std::vector<geometry_msgs::msg::Point> points;
points.push_back(createPoint(-2.0, -10.0, 0.0));
points.push_back(createPoint(2.0, 1.5, 0.0));
points.push_back(createPoint(3.0, 3.0, 0.0));
points.push_back(createPoint(5.0, 10.0, 0.0));
points.push_back(createPoint(10.0, 12.5, 0.0));
points.push_back(create_point(-2.0, -10.0, 0.0));
points.push_back(create_point(2.0, 1.5, 0.0));
points.push_back(create_point(3.0, 3.0, 0.0));
points.push_back(create_point(5.0, 10.0, 0.0));
points.push_back(create_point(10.0, 12.5, 0.0));

SplineInterpolationPoints2d s(points);

Expand Down Expand Up @@ -194,19 +194,19 @@ TEST(spline_interpolation, SplineInterpolationPoints2d)

// size of base_keys is 1 (infeasible to interpolate)
std::vector<geometry_msgs::msg::Point> single_points;
single_points.push_back(createPoint(1.0, 0.0, 0.0));
single_points.push_back(create_point(1.0, 0.0, 0.0));
EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error);
}

TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism)
{
using autoware::universe_utils::createPoint;
using autoware_planning_msgs::msg::TrajectoryPoint;
using autoware_utils::create_point;

std::vector<geometry_msgs::msg::Point> points;
points.push_back(createPoint(-2.0, -10.0, 0.0));
points.push_back(createPoint(2.0, 1.5, 0.0));
points.push_back(createPoint(3.0, 3.0, 0.0));
points.push_back(create_point(-2.0, -10.0, 0.0));
points.push_back(create_point(2.0, 1.5, 0.0));
points.push_back(create_point(3.0, 3.0, 0.0));

std::vector<TrajectoryPoint> trajectory_points;
for (const auto & p : points) {
Expand Down
4 changes: 2 additions & 2 deletions common/autoware_motion_utils/docs/vehicle/vehicle.md
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ bool isVehicleStopped(const double stop_duration)
Necessary includes:

```c++
#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
#include <autoware_utils/vehicle/vehicle_state_checker.hpp>
```

1.Create a checker instance.
Expand Down Expand Up @@ -116,7 +116,7 @@ bool isVehicleStoppedAtStopPoint(const double stop_duration)
Necessary includes:

```c++
#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
#include <autoware_utils/vehicle/vehicle_state_checker.hpp>
```

1.Create a checker instance.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,11 @@
#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_

#include "autoware/universe_utils/system/backtrace.hpp"
#include "autoware_utils/system/backtrace.hpp"

#include <autoware/motion_utils/constants.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware_utils/geometry/geometry.hpp>

#include <vector>

Expand Down 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_utils::get_point(points.at(i));
const auto & next_pt = autoware_utils::get_point(points.at(i + 1));
const double ds = autoware_utils::calc_distance2d(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_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_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_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_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_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_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_utils::print_backtrace();
return false;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_

#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware_utils/geometry/geometry.hpp"

#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
Expand Down 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_utils::get_pose(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_utils::get_pose(points.at(i));
const auto & next_pose = autoware_utils::get_pose(points.at(i + 1));
const double length = autoware_utils::calc_distance3d(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_utils::calc_interpolated_pose(curr_pose, next_pose, ratio);
}
accumulated_length += length;
}

return autoware::universe_utils::getPose(points.back());
return autoware_utils::get_pose(points.back());
}

} // namespace autoware::motion_utils
Expand Down
Loading
Loading