Skip to content

Commit

Permalink
Merge branch 'main' into feauture/pointpainting_multiframe
Browse files Browse the repository at this point in the history
  • Loading branch information
yukke42 authored Aug 22, 2022
2 parents 6b47cc9 + 27f9216 commit 88ce1da
Show file tree
Hide file tree
Showing 177 changed files with 2,182 additions and 2,832 deletions.
2 changes: 1 addition & 1 deletion common/motion_utils/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ Therefore, we recommended using the wrapper utility functions which require the
For example, when we want to calculate the path length between the ego and the dynamic object, the implementation is as follows.

```cpp
const size_t ego_nearest_idx = findFirstNearestSegmentIndex(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
const size_t ego_nearest_seg_idx = findFirstNearestSegmentIndex(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold);
const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx);
```
52 changes: 47 additions & 5 deletions common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
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 Expand Up @@ -1057,6 +1067,38 @@ inline boost::optional<size_t> insertStopPoint(
return stop_idx;
}

template <class T>
void insertOrientation(T & points, const bool is_driving_forward)
{
if (is_driving_forward) {
for (size_t i = 0; i < points.size() - 1; ++i) {
const auto & src_point = tier4_autoware_utils::getPoint(points.at(i));
const auto & dst_point = tier4_autoware_utils::getPoint(points.at(i + 1));
const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point);
const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
tier4_autoware_utils::setOrientation(
tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i));
if (i == points.size() - 2) {
// Terminal orientation is same as the point before it
tier4_autoware_utils::setOrientation(
tier4_autoware_utils::getPose(points.at(i)).orientation, points.at(i + 1));
}
}
} else {
for (size_t i = points.size() - 1; i >= 1; --i) {
const auto & src_point = tier4_autoware_utils::getPoint(points.at(i));
const auto & dst_point = tier4_autoware_utils::getPoint(points.at(i - 1));
const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point);
const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
tier4_autoware_utils::setOrientation(
tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i));
}
// Initial orientation is same as the point after it
tier4_autoware_utils::setOrientation(
tier4_autoware_utils::getPose(points.at(1)).orientation, points.at(0));
}
}

template <class T>
double calcSignedArcLength(
const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx,
Expand Down
2 changes: 1 addition & 1 deletion common/motion_utils/media/ego_nearest_search.svg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
34 changes: 5 additions & 29 deletions common/motion_utils/src/resample/resample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -80,36 +80,12 @@ std::vector<geometry_msgs::msg::Pose> resamplePath(

const bool is_driving_forward =
tier4_autoware_utils::isDrivingForward(points.at(0), points.at(1));
// Insert 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);
}
motion_utils::insertOrientation(resampled_points, is_driving_forward);

// 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;
}
// Initial orientation is depend on the initial value of the resampled_arclength
// when backward driving
if (!is_driving_forward && resampled_arclength.front() < 1e-3) {
resampled_points.at(0).orientation = points.at(0).orientation;
}

return resampled_points;
Expand Down
170 changes: 170 additions & 0 deletions common/motion_utils/test/src/trajectory/test_trajectory.cpp
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
Original file line number Diff line number Diff line change
Expand Up @@ -199,7 +199,7 @@ inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::Tr
}

template <class T>
void setPose(const geometry_msgs::msg::Pose & pose, [[maybe_unused]] T & p)
void setPose([[maybe_unused]] const geometry_msgs::msg::Pose & pose, [[maybe_unused]] T & p)
{
static_assert(sizeof(T) == 0, "Only specializations of getPose can be used.");
throw std::logic_error("Only specializations of getPose can be used.");
Expand Down Expand Up @@ -232,7 +232,15 @@ inline void setPose(
}

template <class T>
void setLongitudinalVelocity(const double velocity, [[maybe_unused]] T & p)
inline void setOrientation(const geometry_msgs::msg::Quaternion & orientation, T & p)
{
auto pose = getPose(p);
pose.orientation = orientation;
setPose(pose, p);
}

template <class T>
void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unused]] T & p)
{
static_assert(sizeof(T) == 0, "Only specializations of getLongitudinalVelocity can be used.");
throw std::logic_error("Only specializations of getLongitudinalVelocity can be used.");
Expand Down
16 changes: 16 additions & 0 deletions common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -288,6 +288,22 @@ TEST(geometry, setPose)
}
}

TEST(geometry, setOrientation)
{
using tier4_autoware_utils::createQuaternionFromRPY;
using tier4_autoware_utils::deg2rad;
using tier4_autoware_utils::setOrientation;

geometry_msgs::msg::Pose p;
const auto orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
setOrientation(orientation, p);

EXPECT_DOUBLE_EQ(p.orientation.x, orientation.x);
EXPECT_DOUBLE_EQ(p.orientation.y, orientation.y);
EXPECT_DOUBLE_EQ(p.orientation.z, orientation.z);
EXPECT_DOUBLE_EQ(p.orientation.w, orientation.w);
}

TEST(geometry, setLongitudinalVelocity)
{
using tier4_autoware_utils::setLongitudinalVelocity;
Expand Down
Loading

0 comments on commit 88ce1da

Please sign in to comment.