Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(motion_utils): add option to use slerp for orientation #1575

Merged
merged 4 commits into from
Aug 22, 2022
Merged

feat(motion_utils): add option to use slerp for orientation #1575

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
24c8f6d
feat(motion_utils): add option to use slerp for orientation
TomohitoAndo Aug 12, 2022
cb2fc82
test: add tests for calcLongitudinalOffsetPose with spherical interpo…
TomohitoAndo Aug 18, 2022
f4935a2
test: modify ratio to be more consitent
TomohitoAndo Aug 18, 2022
f7bf15a
chore: use common flag name
TomohitoAndo Aug 19, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 15 additions & 5 deletions common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -703,11 +703,14 @@ inline boost::optional<geometry_msgs::msg::Point> calcLongitudinalOffsetPoint(
* @param points points of trajectory, path, ...
* @param src_idx index of source point
* @param offset length of offset from source point
* @param set_orientation_from_position_direction set orientation by spherical interpolation if
* false
* @return offset pose
*/
template <class T>
inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
const T & points, const size_t src_idx, const double offset, const bool throw_exception = false)
const T & points, const size_t src_idx, const double offset,
const bool set_orientation_from_position_direction = true, const bool throw_exception = false)
{
try {
validateNonEmpty(points);
Expand Down Expand Up @@ -749,7 +752,8 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
const auto dist_res = -offset - dist_sum;
if (dist_res <= 0.0) {
return tier4_autoware_utils::calcInterpolatedPose(
p_back, p_front, std::abs(dist_res / dist_segment));
p_back, p_front, std::abs(dist_res / dist_segment),
set_orientation_from_position_direction);
}
}
} else {
Expand All @@ -765,7 +769,8 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
const auto dist_res = offset - dist_sum;
if (dist_res <= 0.0) {
return tier4_autoware_utils::calcInterpolatedPose(
p_front, p_back, 1.0 - std::abs(dist_res / dist_segment));
p_front, p_back, 1.0 - std::abs(dist_res / dist_segment),
set_orientation_from_position_direction);
}
}
}
Expand All @@ -779,11 +784,14 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
* @param points points of trajectory, path, ...
* @param src_point source point
* @param offset length of offset from source point
* @param set_orientation_from_position_direction set orientation by spherical interpolation if
* false
* @return offset pase
*/
template <class T>
inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
const T & points, const geometry_msgs::msg::Point & src_point, const double offset)
const T & points, const geometry_msgs::msg::Point & src_point, const double offset,
const bool set_orientation_from_position_direction = true)
{
try {
validateNonEmpty(points);
Expand All @@ -796,7 +804,9 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
const double signed_length_src_offset =
calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point);

return calcLongitudinalOffsetPose(points, src_seg_idx, offset + signed_length_src_offset);
return calcLongitudinalOffsetPose(
points, src_seg_idx, offset + signed_length_src_offset,
set_orientation_from_position_direction);
}

/**
Expand Down
170 changes: 170 additions & 0 deletions common/motion_utils/test/src/trajectory/test_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1312,6 +1312,98 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation)
}
}

TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation)
{
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using motion_utils::calcArcLength;
using motion_utils::calcLongitudinalOffsetPose;
using tier4_autoware_utils::deg2rad;

Trajectory traj{};

{
TrajectoryPoint p;
p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0));
p.longitudinal_velocity_mps = 0.0;
traj.points.push_back(p);
}

{
TrajectoryPoint p;
p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
p.longitudinal_velocity_mps = 0.0;
traj.points.push_back(p);
}

const auto total_length = calcArcLength(traj.points);

// Found pose(forward)
for (double len = 0.0; len < total_length; len += 0.1) {
const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len, false);
// ratio between two points
const auto ratio = len / total_length;
const auto ans_quat =
createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio));

EXPECT_NE(p_out, boost::none);
EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon);
EXPECT_NEAR(p_out.get().position.y, len * std::sin(deg2rad(45.0)), epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon);
}

// Found pose(backward)
for (double len = total_length; 0.0 < len; len -= 0.1) {
const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len, false);
// ratio between two points
const auto ratio = len / total_length;
const auto ans_quat =
createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio));

EXPECT_NE(p_out, boost::none);
EXPECT_NEAR(p_out.get().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon);
EXPECT_NEAR(p_out.get().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon);
}

// Boundary condition
{
const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, false);
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));

EXPECT_NE(p_out, boost::none);
EXPECT_NEAR(p_out.get().position.x, 1.0, 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, ans_quat.x, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon);
}

// Boundary condition
{
const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, false);
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));

EXPECT_NE(p_out, boost::none);
EXPECT_NEAR(p_out.get().position.x, 1.0, 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, ans_quat.x, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon);
}
}

TEST(trajectory, calcLongitudinalOffsetPoseFromPoint)
{
using motion_utils::calcArcLength;
Expand Down Expand Up @@ -1472,6 +1564,84 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation)
}
}

TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation)
{
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using motion_utils::calcArcLength;
using motion_utils::calcLongitudinalOffsetPose;
using motion_utils::calcLongitudinalOffsetToSegment;
using tier4_autoware_utils::createPoint;
using tier4_autoware_utils::deg2rad;

Trajectory traj{};

{
TrajectoryPoint p;
p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0));
p.longitudinal_velocity_mps = 0.0;
traj.points.push_back(p);
}

{
TrajectoryPoint p;
p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
p.longitudinal_velocity_mps = 0.0;
traj.points.push_back(p);
}

const auto total_length = calcArcLength(traj.points);

// Found pose
for (double len_start = 0.0; len_start < total_length; len_start += 0.1) {
constexpr double deviation = 0.1;

const auto p_src = createPoint(
len_start * std::cos(deg2rad(45.0)) + deviation,
len_start * std::sin(deg2rad(45.0)) - deviation, 0.0);
const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src);

for (double len = -src_offset; len < total_length - src_offset; len += 0.1) {
const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len, false);
// ratio between two points
const auto ratio = (src_offset + len) / total_length;
const auto ans_quat =
createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio));

EXPECT_NE(p_out, boost::none);
EXPECT_NEAR(
p_out.get().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon);
EXPECT_NEAR(
p_out.get().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon);
EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon);
EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon);
}
}

// Boundary condition
{
constexpr double deviation = 0.1;

const auto p_src = createPoint(1.0 + deviation, 1.0 - deviation, 0.0);
const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src);

const auto p_out =
calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, false);
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));

EXPECT_NE(p_out, boost::none);
EXPECT_NEAR(p_out.get().position.x, 1.0, 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, ans_quat.x, epsilon);
EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon);
}
}

TEST(trajectory, insertTargetPoint)
{
using motion_utils::calcArcLength;
Expand Down