Skip to content

Commit da60c4e

Browse files
feat: remove dependency on autoware_universe_utils from autoware_interpolation and autoware_motion_utils (autowarefoundation#10092)
Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent b9931c9 commit da60c4e

31 files changed

+908
-917
lines changed

build_depends.repos

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ repositories:
1212
core/autoware_utils:
1313
type: git
1414
url: https://github.com/autowarefoundation/autoware_utils.git
15-
version: 1.0.0
15+
version: 1.1.0
1616
core/autoware_lanelet2_extension:
1717
type: git
1818
url: https://github.com/autowarefoundation/autoware_lanelet2_extension.git

common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
1717

1818
#include "autoware/interpolation/interpolation_utils.hpp"
19-
#include "autoware/universe_utils/geometry/geometry.hpp"
19+
#include "autoware_utils/geometry/geometry.hpp"
2020

2121
#include <Eigen/Core>
2222

common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ class SplineInterpolationPoints2d
4949
{
5050
std::vector<geometry_msgs::msg::Point> points_inner;
5151
for (const auto & p : points) {
52-
points_inner.push_back(autoware::universe_utils::getPoint(p));
52+
points_inner.push_back(autoware_utils::get_point(p));
5353
}
5454
calcSplineCoefficientsInner(points_inner);
5555
}

common/autoware_interpolation/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1212
<buildtool_depend>autoware_cmake</buildtool_depend>
1313

14-
<depend>autoware_universe_utils</depend>
14+
<depend>autoware_utils</depend>
1515
<depend>eigen</depend>
1616

1717
<test_depend>ament_cmake_ros</test_depend>

common/autoware_interpolation/src/spline_interpolation_points_2d.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -89,8 +89,7 @@ geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose(
8989
{
9090
geometry_msgs::msg::Pose pose;
9191
pose.position = getSplineInterpolatedPoint(idx, s);
92-
pose.orientation =
93-
autoware::universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s));
92+
pose.orientation = autoware_utils::create_quaternion_from_yaw(getSplineInterpolatedYaw(idx, s));
9493
return pose;
9594
}
9695

common/autoware_interpolation/test/src/test_spline_interpolation.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
// limitations under the License.
1414

1515
#include "autoware/interpolation/spline_interpolation.hpp"
16-
#include "autoware/universe_utils/geometry/geometry.hpp"
16+
#include "autoware_utils/geometry/geometry.hpp"
1717

1818
#include <gtest/gtest.h>
1919

common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp

+29-29
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414

1515
#include "autoware/interpolation/spline_interpolation.hpp"
1616
#include "autoware/interpolation/spline_interpolation_points_2d.hpp"
17-
#include "autoware/universe_utils/geometry/geometry.hpp"
17+
#include "autoware_utils/geometry/geometry.hpp"
1818

1919
#include <gtest/gtest.h>
2020

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

2828
TEST(spline_interpolation, splineYawFromPoints)
2929
{
30-
using autoware::universe_utils::createPoint;
30+
using autoware_utils::create_point;
3131

3232
{ // straight
3333
std::vector<geometry_msgs::msg::Point> points;
34-
points.push_back(createPoint(0.0, 0.0, 0.0));
35-
points.push_back(createPoint(1.0, 1.5, 0.0));
36-
points.push_back(createPoint(2.0, 3.0, 0.0));
37-
points.push_back(createPoint(3.0, 4.5, 0.0));
38-
points.push_back(createPoint(4.0, 6.0, 0.0));
34+
points.push_back(create_point(0.0, 0.0, 0.0));
35+
points.push_back(create_point(1.0, 1.5, 0.0));
36+
points.push_back(create_point(2.0, 3.0, 0.0));
37+
points.push_back(create_point(3.0, 4.5, 0.0));
38+
points.push_back(create_point(4.0, 6.0, 0.0));
3939

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

@@ -47,11 +47,11 @@ TEST(spline_interpolation, splineYawFromPoints)
4747

4848
{ // curve
4949
std::vector<geometry_msgs::msg::Point> points;
50-
points.push_back(createPoint(-2.0, -10.0, 0.0));
51-
points.push_back(createPoint(2.0, 1.5, 0.0));
52-
points.push_back(createPoint(3.0, 3.0, 0.0));
53-
points.push_back(createPoint(5.0, 10.0, 0.0));
54-
points.push_back(createPoint(10.0, 12.5, 0.0));
50+
points.push_back(create_point(-2.0, -10.0, 0.0));
51+
points.push_back(create_point(2.0, 1.5, 0.0));
52+
points.push_back(create_point(3.0, 3.0, 0.0));
53+
points.push_back(create_point(5.0, 10.0, 0.0));
54+
points.push_back(create_point(10.0, 12.5, 0.0));
5555

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

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

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

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

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

@@ -82,9 +82,9 @@ TEST(spline_interpolation, splineYawFromPoints)
8282

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

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

@@ -97,15 +97,15 @@ TEST(spline_interpolation, splineYawFromPoints)
9797

9898
TEST(spline_interpolation, SplineInterpolationPoints2d)
9999
{
100-
using autoware::universe_utils::createPoint;
100+
using autoware_utils::create_point;
101101

102102
// curve
103103
std::vector<geometry_msgs::msg::Point> points;
104-
points.push_back(createPoint(-2.0, -10.0, 0.0));
105-
points.push_back(createPoint(2.0, 1.5, 0.0));
106-
points.push_back(createPoint(3.0, 3.0, 0.0));
107-
points.push_back(createPoint(5.0, 10.0, 0.0));
108-
points.push_back(createPoint(10.0, 12.5, 0.0));
104+
points.push_back(create_point(-2.0, -10.0, 0.0));
105+
points.push_back(create_point(2.0, 1.5, 0.0));
106+
points.push_back(create_point(3.0, 3.0, 0.0));
107+
points.push_back(create_point(5.0, 10.0, 0.0));
108+
points.push_back(create_point(10.0, 12.5, 0.0));
109109

110110
SplineInterpolationPoints2d s(points);
111111

@@ -194,19 +194,19 @@ TEST(spline_interpolation, SplineInterpolationPoints2d)
194194

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

201201
TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism)
202202
{
203-
using autoware::universe_utils::createPoint;
204203
using autoware_planning_msgs::msg::TrajectoryPoint;
204+
using autoware_utils::create_point;
205205

206206
std::vector<geometry_msgs::msg::Point> points;
207-
points.push_back(createPoint(-2.0, -10.0, 0.0));
208-
points.push_back(createPoint(2.0, 1.5, 0.0));
209-
points.push_back(createPoint(3.0, 3.0, 0.0));
207+
points.push_back(create_point(-2.0, -10.0, 0.0));
208+
points.push_back(create_point(2.0, 1.5, 0.0));
209+
points.push_back(create_point(3.0, 3.0, 0.0));
210210

211211
std::vector<TrajectoryPoint> trajectory_points;
212212
for (const auto & p : points) {

common/autoware_motion_utils/docs/vehicle/vehicle.md

+2-2
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ bool isVehicleStopped(const double stop_duration)
3636
Necessary includes:
3737
3838
```c++
39-
#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
39+
#include <autoware_utils/vehicle/vehicle_state_checker.hpp>
4040
```
4141

4242
1.Create a checker instance.
@@ -116,7 +116,7 @@ bool isVehicleStoppedAtStopPoint(const double stop_duration)
116116
Necessary includes:
117117

118118
```c++
119-
#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
119+
#include <autoware_utils/vehicle/vehicle_state_checker.hpp>
120120
```
121121

122122
1.Create a checker instance.

common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp

+12-12
Original file line numberDiff line numberDiff line change
@@ -15,11 +15,11 @@
1515
#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
1616
#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
1717

18-
#include "autoware/universe_utils/system/backtrace.hpp"
18+
#include "autoware_utils/system/backtrace.hpp"
1919

2020
#include <autoware/motion_utils/constants.hpp>
2121
#include <autoware/motion_utils/trajectory/trajectory.hpp>
22-
#include <autoware/universe_utils/geometry/geometry.hpp>
22+
#include <autoware_utils/geometry/geometry.hpp>
2323

2424
#include <vector>
2525

@@ -50,9 +50,9 @@ template <class T>
5050
bool validate_points_duplication(const T & points)
5151
{
5252
for (size_t i = 0; i < points.size() - 1; ++i) {
53-
const auto & curr_pt = autoware::universe_utils::getPoint(points.at(i));
54-
const auto & next_pt = autoware::universe_utils::getPoint(points.at(i + 1));
55-
const double ds = autoware::universe_utils::calcDistance2d(curr_pt, next_pt);
53+
const auto & curr_pt = autoware_utils::get_point(points.at(i));
54+
const auto & next_pt = autoware_utils::get_point(points.at(i + 1));
55+
const double ds = autoware_utils::calc_distance2d(curr_pt, next_pt);
5656
if (ds < close_s_threshold) {
5757
return false;
5858
}
@@ -67,27 +67,27 @@ bool validate_arguments(const T & input_points, const std::vector<double> & resa
6767
// Check size of the arguments
6868
if (!validate_size(input_points)) {
6969
RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
70-
autoware::universe_utils::print_backtrace();
70+
autoware_utils::print_backtrace();
7171
return false;
7272
}
7373
if (!validate_size(resampling_intervals)) {
7474
RCLCPP_DEBUG(
7575
get_logger(), "invalid argument: The number of resampling intervals is less than 2");
76-
autoware::universe_utils::print_backtrace();
76+
autoware_utils::print_backtrace();
7777
return false;
7878
}
7979

8080
// Check resampling range
8181
if (!validate_resampling_range(input_points, resampling_intervals)) {
8282
RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points");
83-
autoware::universe_utils::print_backtrace();
83+
autoware_utils::print_backtrace();
8484
return false;
8585
}
8686

8787
// Check duplication
8888
if (!validate_points_duplication(input_points)) {
8989
RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
90-
autoware::universe_utils::print_backtrace();
90+
autoware_utils::print_backtrace();
9191
return false;
9292
}
9393

@@ -100,7 +100,7 @@ bool validate_arguments(const T & input_points, const double resampling_interval
100100
// Check size of the arguments
101101
if (!validate_size(input_points)) {
102102
RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
103-
autoware::universe_utils::print_backtrace();
103+
autoware_utils::print_backtrace();
104104
return false;
105105
}
106106

@@ -109,14 +109,14 @@ bool validate_arguments(const T & input_points, const double resampling_interval
109109
RCLCPP_DEBUG(
110110
get_logger(), "invalid argument: resampling interval is less than %f",
111111
autoware::motion_utils::overlap_threshold);
112-
autoware::universe_utils::print_backtrace();
112+
autoware_utils::print_backtrace();
113113
return false;
114114
}
115115

116116
// Check duplication
117117
if (!validate_points_duplication(input_points)) {
118118
RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
119-
autoware::universe_utils::print_backtrace();
119+
autoware_utils::print_backtrace();
120120
return false;
121121
}
122122

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp

+7-7
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
1616
#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
1717

18-
#include "autoware/universe_utils/geometry/geometry.hpp"
18+
#include "autoware_utils/geometry/geometry.hpp"
1919

2020
#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
2121
#include "autoware_planning_msgs/msg/trajectory.hpp"
@@ -73,22 +73,22 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar
7373
}
7474

7575
if (points.size() < 2 || target_length < 0.0) {
76-
return autoware::universe_utils::getPose(points.front());
76+
return autoware_utils::get_pose(points.front());
7777
}
7878

7979
double accumulated_length = 0;
8080
for (size_t i = 0; i < points.size() - 1; ++i) {
81-
const auto & curr_pose = autoware::universe_utils::getPose(points.at(i));
82-
const auto & next_pose = autoware::universe_utils::getPose(points.at(i + 1));
83-
const double length = autoware::universe_utils::calcDistance3d(curr_pose, next_pose);
81+
const auto & curr_pose = autoware_utils::get_pose(points.at(i));
82+
const auto & next_pose = autoware_utils::get_pose(points.at(i + 1));
83+
const double length = autoware_utils::calc_distance3d(curr_pose, next_pose);
8484
if (accumulated_length + length > target_length) {
8585
const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6);
86-
return autoware::universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio);
86+
return autoware_utils::calc_interpolated_pose(curr_pose, next_pose, ratio);
8787
}
8888
accumulated_length += length;
8989
}
9090

91-
return autoware::universe_utils::getPose(points.back());
91+
return autoware_utils::get_pose(points.back());
9292
}
9393

9494
} // namespace autoware::motion_utils

0 commit comments

Comments
 (0)