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 new calcSignedArcLength #1701

Merged
merged 1 commit into from
Aug 26, 2022
Merged

feat(motion_utils): add new calcSignedArcLength #1701

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
28 changes: 28 additions & 0 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 @@ -1115,6 +1115,34 @@ double calcSignedArcLength(
return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
}

template <class T>
double calcSignedArcLength(
const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx,
const size_t dst_idx)
{
validateNonEmpty(points);

const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx);
const double signed_length_src_offset =
calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point);

return signed_length_on_traj - signed_length_src_offset;
}

template <class T>
double calcSignedArcLength(
const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point,
const size_t dst_seg_idx)
{
validateNonEmpty(points);

const double signed_length_on_traj = calcSignedArcLength(points, src_idx, dst_seg_idx);
const double signed_length_dst_offset =
calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point);

return signed_length_on_traj + signed_length_dst_offset;
}

template <class T>
size_t findFirstNearestIndexWithSoftConstraints(
const T & points, const geometry_msgs::msg::Pose & pose,
Expand Down
149 changes: 149 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 @@ -4185,3 +4185,152 @@ TEST(trajectory, findFirstNearestIndexWithSoftConstraints)
}
}
}

TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentIndex)
{
using motion_utils::calcSignedArcLength;

const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);

// Empty
EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0);

// Same point
{
const auto p = createPoint(3.0, 0.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, p, 2), 0, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p, 3, p, 3), 0, epsilon);
}

// Forward
{
const auto p1 = createPoint(0.0, 0.0, 0.0);
const auto p2 = createPoint(3.0, 1.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 2), 3, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 3), 3, epsilon);
}

// Backward
{
const auto p1 = createPoint(9.0, 0.0, 0.0);
const auto p2 = createPoint(8.0, 0.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 7), -1, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 8), -1, epsilon);
}

// Point before start point
{
const auto p1 = createPoint(-3.9, 3.0, 0.0);
const auto p2 = createPoint(6.0, -10.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 5), 9.9, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 6), 9.9, epsilon);
}

// Point after end point
{
const auto p1 = createPoint(7.0, -5.0, 0.0);
const auto p2 = createPoint(13.3, -10.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 6, p2, 8), 6.3, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, p2, 8), 6.3, epsilon);
}

// Point before start point and after end point
{
const auto p1 = createPoint(-4.3, 10.0, 0.0);
const auto p2 = createPoint(13.8, -1.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 18.1, epsilon);
}

// Random cases
{
const auto p1 = createPoint(1.0, 3.0, 0.0);
const auto p2 = createPoint(9.0, -1.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 8, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, p2, 8), 8, epsilon);
}
{
const auto p1 = createPoint(4.3, 7.0, 0.0);
const auto p2 = createPoint(2.0, 3.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 2), -2.3, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 1), -2.3, epsilon);
}
}

TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex)
{
using motion_utils::calcSignedArcLength;

const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);

// Empty
EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0);

// Same point
{
const auto p = createPoint(3.0, 0.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, 3), 0, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 3, p, 3), 0, epsilon);
}

// Forward
{
const auto p1 = createPoint(0.0, 0.0, 0.0);
const auto p2 = createPoint(3.0, 1.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 3), 3, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 2), 3, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 3), 3, epsilon);
}

// Backward
{
const auto p1 = createPoint(9.0, 0.0, 0.0);
const auto p2 = createPoint(8.0, 0.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, 8), -1, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 7), 0, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 8), 0, epsilon);
}

// Point before start point
{
const auto p1 = createPoint(-3.9, 3.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 6), 9.9, epsilon);
}

// Point after end point
{
const auto p2 = createPoint(13.3, -10.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 6.3, epsilon);
}

// Start point
{
const auto p1 = createPoint(0.0, 3.0, 0.0);
const auto p2 = createPoint(5.3, -10.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 5), 5, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 5), 5.3, epsilon);
}

// Point after end point
{
const auto p1 = createPoint(7.3, -5.0, 0.0);
const auto p2 = createPoint(9.0, -10.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, 9), 1.7, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 2.0, epsilon);
}

// Random cases
{
const auto p1 = createPoint(1.0, 3.0, 0.0);
const auto p2 = createPoint(9.0, -1.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 9), 8, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, 9), 8, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 1, p2, 8), 8, epsilon);
}
{
const auto p1 = createPoint(4.3, 7.0, 0.0);
const auto p2 = createPoint(2.3, 3.0, 0.0);
EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, 2), -2.3, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 2), -1.7, epsilon);
EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 1), -1.7, epsilon);
}
}