@@ -48,18 +48,14 @@ PredictedPath PathGenerator::generatePathToTargetPoint(
48
48
const auto velocity = std::max (std::hypot (obj_vel.x , obj_vel.y ), min_crosswalk_user_velocity_);
49
49
const auto arrival_time = pedestrian_to_entry_point.norm () / velocity;
50
50
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
-
55
51
for (double dt = 0.0 ; dt < arrival_time + ep; dt += sampling_time_interval_) {
56
52
geometry_msgs::msg::Pose world_frame_pose;
57
53
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;
59
55
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;
61
57
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 ;
63
59
predicted_path.path .push_back (world_frame_pose);
64
60
if (predicted_path.path .size () >= predicted_path.path .max_size ()) {
65
61
break ;
@@ -91,39 +87,43 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser(
91
87
const auto velocity = std::max (std::hypot (obj_vel.x , obj_vel.y ), min_crosswalk_user_velocity_);
92
88
const auto arrival_time = pedestrian_to_entry_point.norm () / velocity;
93
89
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
-
101
90
for (double dt = 0.0 ; dt < duration + ep; dt += sampling_time_interval_) {
102
91
geometry_msgs::msg::Pose world_frame_pose;
103
92
if (dt < arrival_time) {
104
93
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;
106
95
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;
108
97
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 ;
110
99
predicted_path.path .push_back (world_frame_pose);
111
100
} else {
112
101
world_frame_pose.position .x =
113
102
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);
115
104
world_frame_pose.position .y =
116
105
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);
118
107
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 ;
120
109
predicted_path.path .push_back (world_frame_pose);
121
110
}
122
111
if (predicted_path.path .size () >= predicted_path.path .max_size ()) {
123
112
break ;
124
113
}
125
114
}
126
115
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
+
127
127
predicted_path.confidence = 1.0 ;
128
128
predicted_path.time_step = rclcpp::Duration::from_seconds (sampling_time_interval_);
129
129
0 commit comments