Skip to content

Commit 8e4e052

Browse files
soblinyoutalk
soblin
and
youtalk
authored
feat(trajectory): improve comment, use autoware_pyplot for examples (#282)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
1 parent fcf9ab1 commit 8e4e052

File tree

18 files changed

+128
-59
lines changed

18 files changed

+128
-59
lines changed

Diff for: ‎common/autoware_trajectory/CMakeLists.txt

+10-16
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 3.14)
22

33
project(autoware_trajectory)
44

5-
option(BUILD_EXAMPLES "Build examples" OFF)
65

76
find_package(autoware_cmake REQUIRED)
87
autoware_package()
@@ -21,28 +20,23 @@ if(BUILD_TESTING)
2120
target_link_libraries(test_autoware_trajectory
2221
autoware_trajectory
2322
)
24-
endif()
25-
26-
if(BUILD_EXAMPLES)
27-
message(STATUS "Building examples")
28-
29-
include(FetchContent)
30-
fetchcontent_declare(
31-
matplotlibcpp17
32-
GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git
33-
GIT_TAG master
34-
)
35-
fetchcontent_makeavailable(matplotlibcpp17)
3623

24+
# Examples
3725
file(GLOB_RECURSE example_files examples/*.cpp)
3826

27+
add_definitions("-Wno-attributes")
28+
find_package(ament_lint_auto REQUIRED)
29+
find_package(autoware_pyplot REQUIRED)
30+
ament_lint_auto_find_test_dependencies()
31+
include_directories(${autoware_pyplot_INCLUDE_DIRS})
3932
foreach(example_file ${example_files})
4033
get_filename_component(example_name ${example_file} NAME_WE)
41-
ament_auto_add_executable(${example_name} ${example_file})
42-
set_source_files_properties(${example_file} PROPERTIES COMPILE_FLAGS -Wno-error -Wno-attributes -Wno-unused-parameter)
34+
ament_auto_add_executable(${example_name}
35+
${example_file}
36+
)
4337
target_link_libraries(${example_name}
4438
autoware_trajectory
45-
matplotlibcpp17::matplotlibcpp17
39+
${autoware_pyplot_LIBRARIES}
4640
)
4741
endforeach()
4842
endif()

Diff for: ‎common/autoware_trajectory/examples/example_find_intervals.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,10 @@
1515
#include "autoware/trajectory/path_point_with_lane_id.hpp"
1616
#include "autoware/trajectory/utils/find_intervals.hpp"
1717

18-
#include <matplotlibcpp17/pyplot.h>
18+
#include <autoware/pyplot/pyplot.hpp>
19+
20+
#include <pybind11/embed.h>
21+
#include <pybind11/stl.h>
1922

2023
#include <vector>
2124

@@ -35,7 +38,7 @@ autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_i
3538
int main()
3639
{
3740
pybind11::scoped_interpreter guard{};
38-
auto plt = matplotlibcpp17::pyplot::import();
41+
auto plt = autoware::pyplot::import();
3942

4043
std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> points{
4144
path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0),
@@ -102,6 +105,7 @@ int main()
102105
Args(x_cropped, y_cropped),
103106
Kwargs("color"_a = "red", "label"_a = "interval between lane_id = 1"));
104107

108+
plt.grid();
105109
plt.legend();
106110
plt.show();
107111

Diff for: ‎common/autoware_trajectory/examples/example_interpolator.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -19,15 +19,18 @@
1919
#include "autoware/trajectory/interpolator/nearest_neighbor.hpp"
2020
#include "autoware/trajectory/interpolator/stairstep.hpp"
2121

22-
#include <matplotlibcpp17/pyplot.h>
22+
#include <autoware/pyplot/pyplot.hpp>
23+
24+
#include <pybind11/embed.h>
25+
#include <pybind11/stl.h>
2326

2427
#include <random>
2528
#include <vector>
2629

2730
int main()
2831
{
2932
pybind11::scoped_interpreter guard{};
30-
auto plt = matplotlibcpp17::pyplot::import();
33+
auto plt = autoware::pyplot::import();
3134

3235
// create random values
3336
std::vector<double> bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
@@ -110,6 +113,7 @@ int main()
110113
plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep"));
111114
}
112115

116+
plt.grid();
113117
plt.legend();
114118
plt.show();
115119
return 0;

Diff for: ‎common/autoware_trajectory/examples/example_path_point.cpp

+6-3
Original file line numberDiff line numberDiff line change
@@ -16,13 +16,15 @@
1616
#include "autoware/trajectory/utils/crossed.hpp"
1717
#include "lanelet2_core/primitives/LineString.h"
1818

19+
#include <autoware/pyplot/pyplot.hpp>
20+
1921
#include <autoware_planning_msgs/msg/path_point.hpp>
2022
#include <geometry_msgs/msg/point.hpp>
2123

2224
#include <boost/geometry/geometries/linestring.hpp>
2325

24-
#include <Eigen/src/Core/Matrix.h>
25-
#include <matplotlibcpp17/pyplot.h>
26+
#include <pybind11/embed.h>
27+
#include <pybind11/stl.h>
2628

2729
#include <iostream>
2830
#include <vector>
@@ -42,7 +44,7 @@ int main()
4244

4345
pybind11::scoped_interpreter guard{};
4446

45-
auto plt = matplotlibcpp17::pyplot::import();
47+
auto plt = autoware::pyplot::import();
4648

4749
std::vector<autoware_planning_msgs::msg::PathPoint> points = {
4850
path_point(0.49, 0.59), path_point(0.61, 1.22), path_point(0.86, 1.93), path_point(1.20, 2.56),
@@ -113,6 +115,7 @@ int main()
113115
plt.plot(Args(x_stopped, y_stopped), Kwargs("label"_a = "Stopped", "color"_a = "orange"));
114116

115117
plt.axis(Args("equal"));
118+
plt.grid();
116119
plt.legend();
117120
plt.show();
118121
}

Diff for: ‎common/autoware_trajectory/examples/example_point.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,12 @@
1919
#include "autoware/trajectory/utils/curvature_utils.hpp"
2020
#include "lanelet2_core/primitives/LineString.h"
2121

22+
#include <autoware/pyplot/pyplot.hpp>
23+
2224
#include <geometry_msgs/msg/point.hpp>
2325

24-
#include <matplotlibcpp17/pyplot.h>
26+
#include <pybind11/embed.h>
27+
#include <pybind11/stl.h>
2528

2629
#include <iostream>
2730
#include <vector>
@@ -38,7 +41,7 @@ int main()
3841
{
3942
pybind11::scoped_interpreter guard{};
4043

41-
auto plt = matplotlibcpp17::pyplot::import();
44+
auto plt = autoware::pyplot::import();
4245

4346
std::vector<geometry_msgs::msg::Point> points = {
4447
pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17),
@@ -140,6 +143,7 @@ int main()
140143
}
141144

142145
plt.axis(Args("equal"));
146+
plt.grid();
143147
plt.legend();
144148
plt.show();
145149
}

Diff for: ‎common/autoware_trajectory/examples/example_pose.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,12 @@
1414

1515
#include "autoware/trajectory/pose.hpp"
1616

17+
#include <autoware/pyplot/pyplot.hpp>
18+
1719
#include <geometry_msgs/msg/pose.hpp>
1820

19-
#include <matplotlibcpp17/pyplot.h>
21+
#include <pybind11/embed.h>
22+
#include <pybind11/stl.h>
2023
#include <tf2/LinearMath/Quaternion.h>
2124
#include <tf2/LinearMath/Vector3.h>
2225

@@ -35,7 +38,7 @@ int main()
3538
{
3639
pybind11::scoped_interpreter guard{};
3740

38-
auto plt = matplotlibcpp17::pyplot::import();
41+
auto plt = autoware::pyplot::import();
3942

4043
std::vector<geometry_msgs::msg::Pose> poses = {
4144
pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17),
@@ -87,6 +90,7 @@ int main()
8790
}
8891

8992
plt.axis(Args("equal"));
93+
plt.grid();
9094
plt.legend();
9195
plt.show();
9296
}

Diff for: ‎common/autoware_trajectory/examples/example_shift.cpp

+13-6
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,10 @@
1515
#include "autoware/trajectory/point.hpp"
1616
#include "autoware/trajectory/utils/shift.hpp"
1717

18-
#include <matplotlibcpp17/pyplot.h>
18+
#include <autoware/pyplot/pyplot.hpp>
19+
20+
#include <pybind11/embed.h>
21+
#include <pybind11/stl.h>
1922

2023
#include <iostream>
2124
#include <vector>
@@ -31,7 +34,7 @@ geometry_msgs::msg::Point point(double x, double y)
3134
int main()
3235
{
3336
pybind11::scoped_interpreter guard{};
34-
auto plt = matplotlibcpp17::pyplot::import();
37+
auto plt = autoware::pyplot::import();
3538

3639
std::vector<geometry_msgs::msg::Point> points = {
3740
point(0.49, 0.59), point(0.61, 1.22), point(0.86, 1.93), point(1.20, 2.56), point(1.51, 3.17),
@@ -74,8 +77,9 @@ int main()
7477
y.push_back(p.y);
7578
}
7679

77-
plt.axis(Args("equal"));
7880
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
81+
plt.axis(Args("equal"));
82+
plt.grid();
7983
plt.legend();
8084
plt.show();
8185
}
@@ -105,8 +109,9 @@ int main()
105109
y.push_back(p.y);
106110
}
107111

108-
plt.axis(Args("equal"));
109112
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
113+
plt.axis(Args("equal"));
114+
plt.grid();
110115
plt.legend();
111116
plt.show();
112117
}
@@ -136,8 +141,9 @@ int main()
136141
y.push_back(p.y);
137142
}
138143

139-
plt.axis(Args("equal"));
140144
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
145+
plt.axis(Args("equal"));
146+
plt.grid();
141147
plt.legend();
142148
plt.show();
143149
}
@@ -174,8 +180,9 @@ int main()
174180
y.push_back(p.y);
175181
}
176182

177-
plt.axis(Args("equal"));
178183
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
184+
plt.axis(Args("equal"));
185+
plt.grid();
179186
plt.legend();
180187
plt.show();
181188
}

Diff for: ‎common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp

+6-7
Original file line numberDiff line numberDiff line change
@@ -47,12 +47,12 @@ std::vector<double> merge_vectors(const Vectors &... vectors)
4747
}
4848

4949
/**
50-
* @brief Ensures the vector has at least a specified number of points by linearly interpolating
51-
* values.
50+
* @brief Ensures the output vector has at least a specified number of points by linearly
51+
* interpolating values between each input intervals
5252
*
5353
* @param x Input vector of double values.
54-
* @param min_points Minimum number of points required.
55-
* @return A vector with at least `min_points` elements.
54+
* @param output_size_at_least Minimum number of points required.
55+
* @return A vector of size max(current_size, `output_size_at_least`)
5656
*
5757
* @note If `x.size() >= min_points`, the input vector is returned as-is.
5858
*
@@ -62,10 +62,9 @@ std::vector<double> merge_vectors(const Vectors &... vectors)
6262
* // result: {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}
6363
* @endcode
6464
*/
65-
std::vector<double> fill_bases(const std::vector<double> & x, const size_t & min_points);
65+
std::vector<double> fill_bases(const std::vector<double> & x, const size_t output_size_at_least);
6666

67-
std::vector<double> crop_bases(
68-
const std::vector<double> & x, const double & start, const double & end);
67+
std::vector<double> crop_bases(const std::vector<double> & x, const double start, const double end);
6968
} // namespace helpers
7069
} // namespace autoware::trajectory::detail
7170

Diff for: ‎common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -170,6 +170,7 @@ class InterpolatedArray
170170
}
171171
};
172172

173+
// TODO(soblin): how to insert "linear" or "decelerating" velocity profile ?
173174
/**
174175
* @brief Get a Segment object to set values in a specific range.
175176
* @param start Start of the range.

Diff for: ‎common/autoware_trajectory/include/autoware/trajectory/forward.hpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,9 @@ namespace autoware::trajectory
1919
{
2020

2121
template <typename PointType>
22-
class Trajectory;
22+
class Trajectory
23+
{
24+
};
2325

2426
} // namespace autoware::trajectory
2527

Diff for: ‎common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ trajectory::Trajectory<PointType> shift(
9090
detail::to_point(shifted_points.back()).y -= std::cos(azimuth) * shift_length;
9191
}
9292
auto shifted_trajectory = reference_trajectory;
93-
const bool valid = shifted_trajectory.build(shifted_points);
93+
const auto valid = shifted_trajectory.build(shifted_points);
9494
if (!valid) {
9595
throw std::runtime_error(
9696
"Failed to build shifted trajectory"); // This Exception should not be thrown.

Diff for: ‎common/autoware_trajectory/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
<test_depend>ament_cmake_ros</test_depend>
2626
<test_depend>ament_lint_auto</test_depend>
2727
<test_depend>autoware_lint_common</test_depend>
28+
<test_depend>autoware_pyplot</test_depend>
2829

2930
<export>
3031
<build_type>ament_cmake</build_type>

Diff for: ‎common/autoware_trajectory/src/detail/util.cpp

+12-9
Original file line numberDiff line numberDiff line change
@@ -21,22 +21,26 @@ namespace autoware::trajectory::detail
2121
{
2222
inline namespace helpers
2323
{
24-
std::vector<double> fill_bases(const std::vector<double> & x, const size_t & min_points)
24+
std::vector<double> fill_bases(const std::vector<double> & x, const size_t output_size_at_least)
2525
{
2626
const auto original_size = x.size();
2727

28-
if (original_size >= min_points) {
28+
if (original_size >= output_size_at_least) {
2929
return x;
3030
}
3131

32-
const auto points_to_add = min_points - original_size;
33-
const auto num_gaps = original_size - 1;
32+
// Basically we will insert `new_points_per_interval` to each interval
33+
const auto points_to_add = output_size_at_least - original_size;
34+
const auto num_input_interval = original_size - 1;
35+
const size_t new_points_per_interval = points_to_add / num_input_interval;
3436

35-
std::vector<size_t> points_per_gap(num_gaps, points_to_add / num_gaps);
36-
std::fill_n(points_per_gap.begin(), points_to_add % num_gaps, points_per_gap[0] + 1);
37+
// [`points_to_add / num_gaps`, `points_to_add / num_gaps`, ... `points_to_add / num_gaps`] of
38+
// size `num_gaps`
39+
std::vector<size_t> points_per_gap(num_input_interval, new_points_per_interval);
40+
std::fill_n(points_per_gap.begin(), points_to_add % num_input_interval, points_per_gap[0] + 1);
3741

3842
std::vector<double> result;
39-
result.reserve(min_points);
43+
result.reserve(output_size_at_least);
4044

4145
for (size_t i = 0; i < original_size - 1; ++i) {
4246
result.push_back(x[i]);
@@ -55,8 +59,7 @@ std::vector<double> fill_bases(const std::vector<double> & x, const size_t & min
5559
return result;
5660
}
5761

58-
std::vector<double> crop_bases(
59-
const std::vector<double> & x, const double & start, const double & end)
62+
std::vector<double> crop_bases(const std::vector<double> & x, const double start, const double end)
6063
{
6164
std::vector<double> result;
6265

0 commit comments

Comments
 (0)