Skip to content

Commit 60a9691

Browse files
Merge branch 'beta/v0.20.0' into hotfix/v0.20.0/start_planner
2 parents a068b86 + 2ea2390 commit 60a9691

File tree

15 files changed

+116
-96
lines changed

15 files changed

+116
-96
lines changed

common/interpolation/include/interpolation/spherical_linear_interpolation.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,10 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
3939
const std::vector<double> & base_keys,
4040
const std::vector<geometry_msgs::msg::Quaternion> & base_values,
4141
const std::vector<double> & query_keys);
42+
43+
geometry_msgs::msg::Quaternion lerpOrientation(
44+
const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
45+
const double ratio);
4246
} // namespace interpolation
4347

4448
#endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_

common/interpolation/src/spherical_linear_interpolation.cpp

+11
Original file line numberDiff line numberDiff line change
@@ -57,4 +57,15 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
5757
return query_values;
5858
}
5959

60+
geometry_msgs::msg::Quaternion lerpOrientation(
61+
const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
62+
const double ratio)
63+
{
64+
tf2::Quaternion q_from, q_to;
65+
tf2::fromMsg(o_from, q_from);
66+
tf2::fromMsg(o_to, q_to);
67+
68+
const auto q_interpolated = q_from.slerp(q_to, ratio);
69+
return tf2::toMsg(q_interpolated);
70+
}
6071
} // namespace interpolation

common/motion_utils/include/motion_utils/trajectory/conversion.hpp

+71
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,8 @@
1515
#ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
1616
#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
1717

18+
#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp"
19+
#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
1820
#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp"
1921
#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp"
2022
#include "std_msgs/msg/header.hpp"
@@ -23,6 +25,8 @@
2325

2426
namespace motion_utils
2527
{
28+
using TrajectoryPoints = std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>;
29+
2630
/**
2731
* @brief Convert std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> to
2832
* autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to
@@ -45,6 +49,73 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
4549
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> convertToTrajectoryPointArray(
4650
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
4751

52+
template <class T>
53+
autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input)
54+
{
55+
static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used.");
56+
throw std::logic_error("Only specializations of convertToPath can be used.");
57+
}
58+
59+
template <>
60+
inline autoware_auto_planning_msgs::msg::Path convertToPath(
61+
const autoware_auto_planning_msgs::msg::PathWithLaneId & input)
62+
{
63+
autoware_auto_planning_msgs::msg::Path output{};
64+
output.header = input.header;
65+
output.left_bound = input.left_bound;
66+
output.right_bound = input.right_bound;
67+
output.points.resize(input.points.size());
68+
for (size_t i = 0; i < input.points.size(); ++i) {
69+
output.points.at(i) = input.points.at(i).point;
70+
}
71+
return output;
72+
}
73+
74+
template <class T>
75+
TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input)
76+
{
77+
static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used.");
78+
throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used.");
79+
}
80+
81+
template <>
82+
inline TrajectoryPoints convertToTrajectoryPoints(
83+
const autoware_auto_planning_msgs::msg::PathWithLaneId & input)
84+
{
85+
TrajectoryPoints output{};
86+
for (const auto & p : input.points) {
87+
autoware_auto_planning_msgs::msg::TrajectoryPoint tp;
88+
tp.pose = p.point.pose;
89+
tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
90+
// since path point doesn't have acc for now
91+
tp.acceleration_mps2 = 0;
92+
output.emplace_back(tp);
93+
}
94+
return output;
95+
}
96+
97+
template <class T>
98+
autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
99+
[[maybe_unused]] const T & input)
100+
{
101+
static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used.");
102+
throw std::logic_error("Only specializations of convertToPathWithLaneId can be used.");
103+
}
104+
105+
template <>
106+
inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
107+
const TrajectoryPoints & input)
108+
{
109+
autoware_auto_planning_msgs::msg::PathWithLaneId output{};
110+
for (const auto & p : input) {
111+
autoware_auto_planning_msgs::msg::PathPointWithLaneId pp;
112+
pp.point.pose = p.pose;
113+
pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
114+
output.points.emplace_back(pp);
115+
}
116+
return output;
117+
}
118+
48119
} // namespace motion_utils
49120

50121
#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

+3-9
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@
1616
#define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_
1717

1818
#include "interpolation/linear_interpolation.hpp"
19+
#include "interpolation/spherical_linear_interpolation.hpp"
20+
#include "motion_utils/trajectory/conversion.hpp"
1921
#include "motion_utils/trajectory/trajectory.hpp"
2022
#include "tf2/utils.h"
2123

@@ -74,14 +76,6 @@ Pose calcPoseAfterTimeDelay(
7476
const Pose & current_pose, const double delay_time, const double current_vel,
7577
const double current_acc);
7678

77-
/**
78-
* @brief apply linear interpolation to orientation
79-
* @param [in] o_from first orientation
80-
* @param [in] o_to second orientation
81-
* @param [in] ratio ratio between o_from and o_to for interpolation
82-
*/
83-
Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);
84-
8579
/**
8680
* @brief apply linear interpolation to trajectory point that is nearest to a certain point
8781
* @param [in] points trajectory points
@@ -107,7 +101,7 @@ TrajectoryPoint lerpTrajectoryPoint(
107101
points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
108102
interpolated_point.pose.position.y = interpolation::lerp(
109103
points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
110-
interpolated_point.pose.orientation = lerpOrientation(
104+
interpolated_point.pose.orientation = interpolation::lerpOrientation(
111105
points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
112106
interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
113107
points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps,

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

-10
Original file line numberDiff line numberDiff line change
@@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio)
141141
return v_from + (v_to - v_from) * ratio;
142142
}
143143

144-
Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
145-
{
146-
tf2::Quaternion q_from, q_to;
147-
tf2::fromMsg(o_from, q_from);
148-
tf2::fromMsg(o_to, q_to);
149-
150-
const auto q_interpolated = q_from.slerp(q_to, ratio);
151-
return tf2::toMsg(q_interpolated);
152-
}
153-
154144
double applyDiffLimitFilter(
155145
const double input_val, const double prev_val, const double dt, const double max_val,
156146
const double min_val)

control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp

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

1515
#include "gtest/gtest.h"
16+
#include "interpolation/spherical_linear_interpolation.hpp"
17+
#include "motion_utils/trajectory/conversion.hpp"
1618
#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp"
1719
#include "tf2/LinearMath/Quaternion.h"
1820

@@ -303,15 +305,15 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
303305
o_to.setRPY(M_PI_4, M_PI_4, M_PI_4);
304306

305307
ratio = 0.0;
306-
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
308+
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
307309
tf2::convert(result, o_result);
308310
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
309311
EXPECT_DOUBLE_EQ(roll, 0.0);
310312
EXPECT_DOUBLE_EQ(pitch, 0.0);
311313
EXPECT_DOUBLE_EQ(yaw, 0.0);
312314

313315
ratio = 1.0;
314-
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
316+
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
315317
tf2::convert(result, o_result);
316318
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
317319
EXPECT_DOUBLE_EQ(roll, M_PI_4);
@@ -320,23 +322,23 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
320322

321323
ratio = 0.5;
322324
o_to.setRPY(M_PI_4, 0.0, 0.0);
323-
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
325+
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
324326
tf2::convert(result, o_result);
325327
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
326328
EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2);
327329
EXPECT_DOUBLE_EQ(pitch, 0.0);
328330
EXPECT_DOUBLE_EQ(yaw, 0.0);
329331

330332
o_to.setRPY(0.0, M_PI_4, 0.0);
331-
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
333+
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
332334
tf2::convert(result, o_result);
333335
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
334336
EXPECT_DOUBLE_EQ(roll, 0.0);
335337
EXPECT_DOUBLE_EQ(pitch, M_PI_4 / 2);
336338
EXPECT_DOUBLE_EQ(yaw, 0.0);
337339

338340
o_to.setRPY(0.0, 0.0, M_PI_4);
339-
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
341+
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
340342
tf2::convert(result, o_result);
341343
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
342344
EXPECT_DOUBLE_EQ(roll, 0.0);

planning/behavior_path_lane_change_module/src/interface.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -276,6 +276,11 @@ bool LaneChangeInterface::canTransitFailureState()
276276
return false;
277277
}
278278

279+
if (!module_type_->isAbleToReturnCurrentLane()) {
280+
log_debug_throttled("It's is not possible to return to original lane. Continue lane change.");
281+
return false;
282+
}
283+
279284
const auto found_abort_path = module_type_->calcAbortPath();
280285
if (!found_abort_path) {
281286
log_debug_throttled(

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#include "behavior_path_planner_common/marker_utils/utils.hpp"
1818
#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
1919
#include "behavior_path_planner_common/utils/path_utils.hpp"
20+
#include "motion_utils/trajectory/conversion.hpp"
2021

2122
#include <tier4_autoware_utils/ros/update_param.hpp>
2223
#include <vehicle_info_util/vehicle_info_util.hpp>
@@ -700,7 +701,8 @@ Path BehaviorPathPlannerNode::convertToPath(
700701
return output;
701702
}
702703

703-
output = utils::toPath(*path_candidate_ptr);
704+
output = motion_utils::convertToPath<autoware_auto_planning_msgs::msg::PathWithLaneId>(
705+
*path_candidate_ptr);
704706
// header is replaced by the input one, so it is substituted again
705707
output.header = planner_data->route_handler->getRouteHeader();
706708
output.header.stamp = this->now();

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -58,8 +58,6 @@ PathWithLaneId resamplePathWithSpline(
5858
const PathWithLaneId & path, const double interval, const bool keep_input_points = false,
5959
const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});
6060

61-
Path toPath(const PathWithLaneId & input);
62-
6361
size_t getIdxByArclength(
6462
const PathWithLaneId & path, const size_t target_idx, const double signed_arc);
6563

planning/behavior_path_planner_common/src/utils/path_utils.cpp

-13
Original file line numberDiff line numberDiff line change
@@ -155,19 +155,6 @@ PathWithLaneId resamplePathWithSpline(
155155
return motion_utils::resamplePath(path, s_out);
156156
}
157157

158-
Path toPath(const PathWithLaneId & input)
159-
{
160-
Path output{};
161-
output.header = input.header;
162-
output.left_bound = input.left_bound;
163-
output.right_bound = input.right_bound;
164-
output.points.resize(input.points.size());
165-
for (size_t i = 0; i < input.points.size(); ++i) {
166-
output.points.at(i) = input.points.at(i).point;
167-
}
168-
return output;
169-
}
170-
171158
size_t getIdxByArclength(
172159
const PathWithLaneId & path, const size_t target_idx, const double signed_arc)
173160
{

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp

-5
Original file line numberDiff line numberDiff line change
@@ -35,11 +35,6 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
3535
using geometry_msgs::msg::Quaternion;
3636
using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;
3737

38-
TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path);
39-
PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory);
40-
41-
Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);
42-
4338
//! smooth path point with lane id starts from ego position on path to the path end
4439
bool smoothPath(
4540
const PathWithLaneId & in_path, PathWithLaneId & out_path,

planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp

+6-37
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,8 @@
1313
// limitations under the License.
1414

1515
// #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
16+
#include "motion_utils/trajectory/conversion.hpp"
17+
1618
#include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
1719
#include <motion_utils/trajectory/trajectory.hpp>
1820
#include <motion_velocity_smoother/trajectory_utils.hpp>
@@ -41,41 +43,6 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
4143
using geometry_msgs::msg::Quaternion;
4244
using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;
4345

44-
TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path)
45-
{
46-
TrajectoryPoints tps;
47-
for (const auto & p : path.points) {
48-
TrajectoryPoint tp;
49-
tp.pose = p.point.pose;
50-
tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
51-
// since path point doesn't have acc for now
52-
tp.acceleration_mps2 = 0;
53-
tps.emplace_back(tp);
54-
}
55-
return tps;
56-
}
57-
PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory)
58-
{
59-
PathWithLaneId path;
60-
for (const auto & p : trajectory) {
61-
PathPointWithLaneId pp;
62-
pp.point.pose = p.pose;
63-
pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
64-
path.points.emplace_back(pp);
65-
}
66-
return path;
67-
}
68-
69-
Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
70-
{
71-
tf2::Quaternion q_from, q_to;
72-
tf2::fromMsg(o_from, q_from);
73-
tf2::fromMsg(o_to, q_to);
74-
75-
const auto q_interpolated = q_from.slerp(q_to, ratio);
76-
return tf2::toMsg(q_interpolated);
77-
}
78-
7946
//! smooth path point with lane id starts from ego position on path to the path end
8047
bool smoothPath(
8148
const PathWithLaneId & in_path, PathWithLaneId & out_path,
@@ -87,7 +54,9 @@ bool smoothPath(
8754
const auto & external_v_limit = planner_data->external_velocity_limit;
8855
const auto & smoother = planner_data->velocity_smoother_;
8956

90-
auto trajectory = convertPathToTrajectoryPoints(in_path);
57+
auto trajectory =
58+
motion_utils::convertToTrajectoryPoints<autoware_auto_planning_msgs::msg::PathWithLaneId>(
59+
in_path);
9160
const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
9261

9362
const auto traj_steering_rate_limited =
@@ -117,7 +86,7 @@ bool smoothPath(
11786
motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
11887
traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed);
11988
}
120-
out_path = convertTrajectoryPointsToPath(traj_smoothed);
89+
out_path = motion_utils::convertToPathWithLaneId<TrajectoryPoints>(traj_smoothed);
12190
return true;
12291
}
12392

planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp

-13
Original file line numberDiff line numberDiff line change
@@ -492,19 +492,6 @@ void updateNodeOptions(
492492
node_options.arguments(std::vector<std::string>{arguments.begin(), arguments.end()});
493493
}
494494

495-
Path toPath(const PathWithLaneId & input)
496-
{
497-
Path output{};
498-
output.header = input.header;
499-
output.left_bound = input.left_bound;
500-
output.right_bound = input.right_bound;
501-
output.points.resize(input.points.size());
502-
for (size_t i = 0; i < input.points.size(); ++i) {
503-
output.points.at(i) = input.points.at(i).point;
504-
}
505-
return output;
506-
}
507-
508495
PathWithLaneId loadPathWithLaneIdInYaml()
509496
{
510497
const auto planning_test_utils_dir =

planning/planning_test_utils/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
<depend>component_interface_utils</depend>
2424
<depend>lanelet2_extension</depend>
2525
<depend>lanelet2_io</depend>
26+
<depend>motion_utils</depend>
2627
<depend>nav_msgs</depend>
2728
<depend>rclcpp</depend>
2829
<depend>route_handler</depend>

0 commit comments

Comments
 (0)