Skip to content

Commit

Permalink
feat(motion_utils): add backward checker for resampling function (#1488)
Browse files Browse the repository at this point in the history 
Signed-off-by: yutaka <purewater0901@gmail.com>
  • Loading branch information
@purewater0901
purewater0901 committed Aug 2, 2022
1 parent 8a8c754 commit 08d7a12
Show file tree
Hide file tree
Showing 2 changed files with 210 additions and 10 deletions.
42 changes: 32 additions & 10 deletions common/motion_utils/src/resample/resample.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include "motion_utils/resample/resample.hpp"

#include "tier4_autoware_utils/geometry/geometry.hpp"

namespace motion_utils
{
std::vector<geometry_msgs::msg::Pose> resamplePath(
Expand Down Expand Up @@ -76,17 +78,37 @@ std::vector<geometry_msgs::msg::Pose> resamplePath(
resampled_points.at(i) = pose;
}

const bool is_driving_forward =
tier4_autoware_utils::isDrivingForward(points.at(0), points.at(1));
// Insert Orientation
for (size_t i = 0; i < resampled_points.size() - 1; ++i) {
const auto & src_point = resampled_points.at(i).position;
const auto & dst_point = resampled_points.at(i + 1).position;
const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point);
const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
resampled_points.at(i).orientation =
tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
if (i == resampled_points.size() - 2) {
// Terminal Orientation is same as the point before it
resampled_points.at(i + 1).orientation = resampled_points.at(i).orientation;
if (is_driving_forward) {
for (size_t i = 0; i < resampled_points.size() - 1; ++i) {
const auto & src_point = resampled_points.at(i).position;
const auto & dst_point = resampled_points.at(i + 1).position;
const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point);
const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
resampled_points.at(i).orientation =
tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
if (i == resampled_points.size() - 2) {
// Terminal Orientation is same as the point before it
resampled_points.at(i + 1).orientation = resampled_points.at(i).orientation;
}
}
} else {
for (size_t i = resampled_points.size() - 1; i >= 1; --i) {
const auto & src_point = resampled_points.at(i).position;
const auto & dst_point = resampled_points.at(i - 1).position;
const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point);
const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
resampled_points.at(i).orientation =
tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
}

// Initial Orientation is depend on the initial value of the resampled_arclength
if (resampled_arclength.front() < 1e-3) {
resampled_points.at(0).orientation = points.at(0).orientation;
} else {
resampled_points.at(0).orientation = resampled_points.at(1).orientation;
}
}

Expand Down
178 changes: 178 additions & 0 deletions common/motion_utils/test/src/resample/test_resample.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -380,6 +380,184 @@ TEST(resample_path, resample_path_by_vector)
}
}

TEST(resample_path, resample_path_by_vector_backward)
{
using motion_utils::resamplePath;

{
autoware_auto_planning_msgs::msg::Path path;
path.points.resize(10);
for (size_t i = 0; i < 10; ++i) {
path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1);
}
std::vector<double> resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0};

const auto resampled_path = resamplePath(path, resampled_arclength);

{
const auto p = resampled_path.points.at(0);
EXPECT_NEAR(p.pose.position.x, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon);
}

{
const auto p = resampled_path.points.at(1);
EXPECT_NEAR(p.pose.position.x, 1.2, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon);
}

{
const auto p = resampled_path.points.at(2);
EXPECT_NEAR(p.pose.position.x, 1.5, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon);
}

{
const auto p = resampled_path.points.at(3);
EXPECT_NEAR(p.pose.position.x, 5.3, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon);
}

{
const auto p = resampled_path.points.at(4);
EXPECT_NEAR(p.pose.position.x, 7.5, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon);
}

{
const auto p = resampled_path.points.at(5);
EXPECT_NEAR(p.pose.position.x, 9.0, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon);
}

const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI);
for (size_t i = 0; i < resampled_path.points.size(); ++i) {
const auto p = resampled_path.points.at(i);
EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon);
}
}

// change initial orientation
{
autoware_auto_planning_msgs::msg::Path path;
path.points.resize(10);
for (size_t i = 0; i < 10; ++i) {
path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1);
}
path.points.at(0).pose.orientation =
tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0);
std::vector<double> resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0};

const auto resampled_path = resamplePath(path, resampled_arclength);

{
const auto p = resampled_path.points.at(0);
EXPECT_NEAR(p.pose.position.x, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon);
}

{
const auto p = resampled_path.points.at(1);
EXPECT_NEAR(p.pose.position.x, 1.2, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon);
}

{
const auto p = resampled_path.points.at(2);
EXPECT_NEAR(p.pose.position.x, 1.5, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon);
}

{
const auto p = resampled_path.points.at(3);
EXPECT_NEAR(p.pose.position.x, 5.3, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon);
}

{
const auto p = resampled_path.points.at(4);
EXPECT_NEAR(p.pose.position.x, 7.5, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon);
}

{
const auto p = resampled_path.points.at(5);
EXPECT_NEAR(p.pose.position.x, 9.0, epsilon);
EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon);
EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon);
EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon);
}

// Initial Orientation
{
const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0);
const auto p = resampled_path.points.at(0);
EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon);
}

const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI);
for (size_t i = 1; i < resampled_path.points.size(); ++i) {
const auto p = resampled_path.points.at(i);
EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon);
}
}
}

TEST(resample_path, resample_path_by_vector_non_default)
{
using motion_utils::resamplePath;
Expand Down

0 comments on commit 08d7a12

Please sign in to comment.