Skip to content

Commit

Permalink
feat(motion_utils): add option to use slerp for orientation (autoware…
Browse files Browse the repository at this point in the history 
…foundation#1575)

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

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* test: add tests for calcLongitudinalOffsetPose with spherical interpolation

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* test: modify ratio to be more consitent

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* chore: use common flag name

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
  • Loading branch information
@TomohitoAndo
TomohitoAndo authored and yukke42 committed Oct 14, 2022
1 parent 975bbee commit 38c9264
Show file tree
Hide file tree
Showing 2 changed files with 185 additions and 5 deletions.
20 changes: 15 additions & 5 deletions common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
View file
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
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

0 comments on commit 38c9264

Please sign in to comment.