Skip to content

Commit 0a56e0e

Browse files
committed
Revert "fix(map_based_prediction): improve pedestrian path generation efficiency (#6964)"
This reverts commit c7c826d.
1 parent 2877834 commit 0a56e0e

File tree

2 files changed

+21
-21
lines changed

2 files changed

+21
-21
lines changed

perception/map_based_prediction/src/map_based_prediction_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -1145,7 +1145,7 @@ bool MapBasedPredictionNode::doesPathCrossFence(
11451145
const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line)
11461146
{
11471147
// check whether the predicted path cross with fence
1148-
for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) {
1148+
for (size_t i = 0; i < predicted_path.path.size(); ++i) {
11491149
for (size_t j = 0; j < fence_line.size() - 1; ++j) {
11501150
if (isIntersecting(
11511151
predicted_path.path[i].position, predicted_path.path[i + 1].position, fence_line[j],

perception/map_based_prediction/src/path_generator.cpp

+20-20
Original file line numberDiff line numberDiff line change
@@ -48,18 +48,14 @@ PredictedPath PathGenerator::generatePathToTargetPoint(
4848
const auto velocity = std::max(std::hypot(obj_vel.x, obj_vel.y), min_crosswalk_user_velocity_);
4949
const auto arrival_time = pedestrian_to_entry_point.norm() / velocity;
5050

51-
const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized();
52-
const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw(
53-
std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x()));
54-
5551
for (double dt = 0.0; dt < arrival_time + ep; dt += sampling_time_interval_) {
5652
geometry_msgs::msg::Pose world_frame_pose;
5753
world_frame_pose.position.x =
58-
obj_pos.x + velocity * pedestrian_to_entry_point_normalized.x() * dt;
54+
obj_pos.x + velocity * pedestrian_to_entry_point.normalized().x() * dt;
5955
world_frame_pose.position.y =
60-
obj_pos.y + velocity * pedestrian_to_entry_point_normalized.y() * dt;
56+
obj_pos.y + velocity * pedestrian_to_entry_point.normalized().y() * dt;
6157
world_frame_pose.position.z = obj_pos.z;
62-
world_frame_pose.orientation = pedestrian_to_entry_point_orientation;
58+
world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation;
6359
predicted_path.path.push_back(world_frame_pose);
6460
if (predicted_path.path.size() >= predicted_path.path.max_size()) {
6561
break;
@@ -91,39 +87,43 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser(
9187
const auto velocity = std::max(std::hypot(obj_vel.x, obj_vel.y), min_crosswalk_user_velocity_);
9288
const auto arrival_time = pedestrian_to_entry_point.norm() / velocity;
9389

94-
const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized();
95-
const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw(
96-
std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x()));
97-
const auto entry_to_exit_point_normalized = entry_to_exit_point.normalized();
98-
const auto entry_to_exit_point_orientation = tier4_autoware_utils::createQuaternionFromYaw(
99-
std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x()));
100-
10190
for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) {
10291
geometry_msgs::msg::Pose world_frame_pose;
10392
if (dt < arrival_time) {
10493
world_frame_pose.position.x =
105-
obj_pos.x + velocity * pedestrian_to_entry_point_normalized.x() * dt;
94+
obj_pos.x + velocity * pedestrian_to_entry_point.normalized().x() * dt;
10695
world_frame_pose.position.y =
107-
obj_pos.y + velocity * pedestrian_to_entry_point_normalized.y() * dt;
96+
obj_pos.y + velocity * pedestrian_to_entry_point.normalized().y() * dt;
10897
world_frame_pose.position.z = obj_pos.z;
109-
world_frame_pose.orientation = pedestrian_to_entry_point_orientation;
98+
world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation;
11099
predicted_path.path.push_back(world_frame_pose);
111100
} else {
112101
world_frame_pose.position.x =
113102
reachable_crosswalk.front_center_point.x() +
114-
velocity * entry_to_exit_point_normalized.x() * (dt - arrival_time);
103+
velocity * entry_to_exit_point.normalized().x() * (dt - arrival_time);
115104
world_frame_pose.position.y =
116105
reachable_crosswalk.front_center_point.y() +
117-
velocity * entry_to_exit_point_normalized.y() * (dt - arrival_time);
106+
velocity * entry_to_exit_point.normalized().y() * (dt - arrival_time);
118107
world_frame_pose.position.z = obj_pos.z;
119-
world_frame_pose.orientation = entry_to_exit_point_orientation;
108+
world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation;
120109
predicted_path.path.push_back(world_frame_pose);
121110
}
122111
if (predicted_path.path.size() >= predicted_path.path.max_size()) {
123112
break;
124113
}
125114
}
126115

116+
// calculate orientation of each point
117+
if (predicted_path.path.size() >= 2) {
118+
for (size_t i = 0; i < predicted_path.path.size() - 1; i++) {
119+
const auto yaw = tier4_autoware_utils::calcAzimuthAngle(
120+
predicted_path.path.at(i).position, predicted_path.path.at(i + 1).position);
121+
predicted_path.path.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
122+
}
123+
predicted_path.path.back().orientation =
124+
predicted_path.path.at(predicted_path.path.size() - 2).orientation;
125+
}
126+
127127
predicted_path.confidence = 1.0;
128128
predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_);
129129

0 commit comments

Comments
 (0)