Skip to content

Commit

Permalink
fix(tier4_autoware_utils): fix interpolated point pose
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Jun 20, 2022
1 parent 56990ce commit c965453
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 8 deletions.
21 changes: 17 additions & 4 deletions common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(

const auto dist_res = offset - dist_sum;
if (dist_res <= 0.0) {
return calcInterpolatedPose(p_back, p_front, std::abs(dist_res / dist_segment));
return calcInterpolatedPose(p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), false);
}
}

Expand Down Expand Up @@ -118,21 +118,34 @@ inline size_t insertTargetPoint(

const auto p_front = getPoint(points.at(seg_idx));
const auto p_back = getPoint(points.at(seg_idx + 1));
const auto yaw = calcAzimuthAngle(p_target, p_back);

validateNonSharpAngle(p_front, p_target, p_back);

const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold;
const auto overlap_with_back = calcDistance2d(p_target, p_back) < overlap_threshold;

geometry_msgs::msg::Pose target_pose;
target_pose.position = p_target;
target_pose.orientation = createQuaternionFromYaw(yaw);
{
const auto pitch = calcElevationAngle(p_target, p_back);
const auto yaw = calcAzimuthAngle(p_target, p_back);

target_pose.position = p_target;
target_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
}

auto p_insert = points.at(seg_idx);
setPose(target_pose, p_insert);

geometry_msgs::msg::Pose front_pose;
{
const auto pitch = calcElevationAngle(p_front, p_target);
const auto yaw = calcAzimuthAngle(p_front, p_target);

front_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
}

if (!overlap_with_front && !overlap_with_back) {
setPose(front_pose, points.at(seg_idx));
points.insert(points.begin() + seg_idx + 1, p_insert);
return seg_idx + 1;
}
Expand Down
21 changes: 17 additions & 4 deletions common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -426,7 +426,7 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(

const auto dist_res = offset - dist_sum;
if (dist_res <= 0.0) {
return calcInterpolatedPose(p_back, p_front, std::abs(dist_res / dist_segment));
return calcInterpolatedPose(p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), false);
}
}

Expand Down Expand Up @@ -475,21 +475,34 @@ inline size_t insertTargetPoint(

const auto p_front = getPoint(points.at(seg_idx));
const auto p_back = getPoint(points.at(seg_idx + 1));
const auto yaw = calcAzimuthAngle(p_target, p_back);

validateNonSharpAngle(p_front, p_target, p_back);

const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold;
const auto overlap_with_back = calcDistance2d(p_target, p_back) < overlap_threshold;

geometry_msgs::msg::Pose target_pose;
target_pose.position = p_target;
target_pose.orientation = createQuaternionFromYaw(yaw);
{
const auto pitch = calcElevationAngle(p_target, p_back);
const auto yaw = calcAzimuthAngle(p_target, p_back);

target_pose.position = p_target;
target_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
}

auto p_insert = points.at(seg_idx);
setPose(target_pose, p_insert);

geometry_msgs::msg::Pose front_pose;
{
const auto pitch = calcElevationAngle(p_front, p_target);
const auto yaw = calcAzimuthAngle(p_front, p_target);

front_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
}

if (!overlap_with_front && !overlap_with_back) {
setPose(front_pose, points.at(seg_idx));
points.insert(points.begin() + seg_idx + 1, p_insert);
return seg_idx + 1;
}
Expand Down
40 changes: 40 additions & 0 deletions common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -901,6 +901,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 3.0, epsilon);
EXPECT_NEAR(p_out.get().position.y, 2.0, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Whole length
Expand All @@ -911,6 +915,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Forward offset
Expand All @@ -921,6 +929,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 2.707106781186547524, epsilon);
EXPECT_NEAR(p_out.get().position.y, 1.707106781186547524, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Backward offset
Expand All @@ -931,6 +943,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 0.41421356237309505, epsilon);
EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}
}

Expand Down Expand Up @@ -1128,6 +1144,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 3.0, epsilon);
EXPECT_NEAR(p_out.get().position.y, 2.0, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Whole length
Expand All @@ -1139,6 +1159,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Forward offset(No lateral deviation)
Expand All @@ -1150,6 +1174,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 2.707106781186547524, epsilon);
EXPECT_NEAR(p_out.get().position.y, 1.707106781186547524, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Forward offset(Lateral deviation)
Expand All @@ -1161,6 +1189,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 1.25, epsilon);
EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Backward offset(No lateral deviation)
Expand All @@ -1172,6 +1204,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 0.41421356237309505, epsilon);
EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}

// Backward offset(No lateral deviation)
Expand All @@ -1183,6 +1219,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_CurveTrajectory)
EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon);
EXPECT_NEAR(p_out.get().position.y, 0.75, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon);
}
}

Expand Down

0 comments on commit c965453

Please sign in to comment.