Skip to content

Commit

Permalink
feat(motion_utils): fix orientaion (tier4#1397)
Browse files Browse the repository at this point in the history
  • Loading branch information
@purewater0901 @boyali
purewater0901 authored and boyali committed Sep 28, 2022
1 parent f033e0f commit 57ce89a
Show file tree
Hide file tree
Showing 3 changed files with 114 additions and 39 deletions.
93 changes: 55 additions & 38 deletions common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,45 @@ void validateNonSharpAngle(
}
}

template <class T>
bool isDrivingForward(const T points)
{
if (points.size() < 2) {
return true;
}

// check the first point direction
const auto & first_point_pose = tier4_autoware_utils::getPose(points.at(0));
const auto & second_point_pose = tier4_autoware_utils::getPose(points.at(1));

const double first_point_yaw = tf2::getYaw(first_point_pose.orientation);
const double driving_direction_yaw =
tier4_autoware_utils::calcAzimuthAngle(first_point_pose.position, second_point_pose.position);
if (
std::abs(tier4_autoware_utils::normalizeRadian(first_point_yaw - driving_direction_yaw)) <
tier4_autoware_utils::pi / 2.0) {
return true;
}

return false;
}

template <class T>
bool isDrivingForwardWithTwist(const T points_with_twist)
{
if (points_with_twist.empty()) {
return true;
}
if (points_with_twist.size() == 1) {
if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
return true;
}
return false;
}

return isDrivingForward(points_with_twist);
}

template <class T>
boost::optional<size_t> searchZeroVelocityIndex(
const T & points_with_twist, const size_t src_idx, const size_t dst_idx)
Expand Down Expand Up @@ -805,10 +844,13 @@ inline boost::optional<size_t> insertTargetPoint(
const auto overlap_with_back =
tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold;

const bool is_driving_forward = isDrivingForward(points);

geometry_msgs::msg::Pose target_pose;
{
const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_back);
const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_back);
const auto p_base = is_driving_forward ? p_back : p_front;
const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base);
const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base);

target_pose.position = p_target;
target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
Expand All @@ -817,17 +859,22 @@ inline boost::optional<size_t> insertTargetPoint(
auto p_insert = points.at(seg_idx);
tier4_autoware_utils::setPose(target_pose, p_insert);

geometry_msgs::msg::Pose front_pose;
geometry_msgs::msg::Pose base_pose;
{
const auto pitch = tier4_autoware_utils::calcElevationAngle(p_front, p_target);
const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_target);
const auto p_base = is_driving_forward ? p_front : p_back;
const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target);
const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target);

front_pose.position = tier4_autoware_utils::getPoint(points.at(seg_idx));
front_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
base_pose.position = tier4_autoware_utils::getPoint(p_base);
base_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
}

if (!overlap_with_front && !overlap_with_back) {
tier4_autoware_utils::setPose(front_pose, points.at(seg_idx));
if (is_driving_forward) {
tier4_autoware_utils::setPose(base_pose, points.at(seg_idx));
} else {
tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1));
}
points.insert(points.begin() + seg_idx + 1, p_insert);
return seg_idx + 1;
}
Expand Down Expand Up @@ -1012,36 +1059,6 @@ inline boost::optional<size_t> insertStopPoint(

return stop_idx;
}

template <class T>
inline bool isDrivingForward(const T points_with_twist)
{
// if points size is smaller than 2
if (points_with_twist.empty()) {
return true;
}
if (points_with_twist.size() == 1) {
if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
return true;
}
return false;
}

// check the first point direction
const auto & first_point_pose = tier4_autoware_utils::getPose(points_with_twist.at(0));
const auto & second_point_pose = tier4_autoware_utils::getPose(points_with_twist.at(1));

const double first_point_yaw = tf2::getYaw(first_point_pose.orientation);
const double driving_direction_yaw =
tier4_autoware_utils::calcAzimuthAngle(first_point_pose.position, second_point_pose.position);
if (
std::abs(tier4_autoware_utils::normalizeRadian(first_point_yaw - driving_direction_yaw)) <
M_PI_2) {
return true;
}

return false;
}
} // namespace motion_utils

#endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_
58 changes: 58 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 @@ -1691,6 +1691,64 @@ TEST(trajectory, insertTargetPoint)
}
}

TEST(trajectory, insertTargetPoint_Reverse)
{
using motion_utils::calcArcLength;
using motion_utils::findNearestSegmentIndex;
using motion_utils::insertTargetPoint;
using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::createPoint;
using tier4_autoware_utils::createQuaternionFromYaw;
using tier4_autoware_utils::deg2rad;
using tier4_autoware_utils::getPose;

constexpr double overlap_threshold = 1e-4;
auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
for (size_t i = 0; i < traj.points.size(); ++i) {
traj.points.at(i).pose.orientation = createQuaternionFromYaw(tier4_autoware_utils::pi);
}
const auto total_length = calcArcLength(traj.points);

for (double x_start = 0.0; x_start < total_length; x_start += 1.0) {
auto traj_out = traj;

const auto p_target = createPoint(x_start + 1.1e-4, 0.0, 0.0);
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx =
insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);

EXPECT_NE(insert_idx, boost::none);
EXPECT_EQ(insert_idx.get(), base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);

for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
EXPECT_TRUE(
calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold);
}

{
const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180));
EXPECT_EQ(p_insert.position.x, p_target.x);
EXPECT_EQ(p_insert.position.y, p_target.y);
EXPECT_EQ(p_insert.position.z, p_target.z);
EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
}

{
const auto p_base = getPose(traj_out.points.at(base_idx));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180));
EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
}
}
}

TEST(trajectory, insertTargetPoint_OverlapThreshold)
{
using motion_utils::calcArcLength;
Expand Down
2 changes: 1 addition & 1 deletion planning/obstacle_cruise_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -493,7 +493,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms

// Get Target Obstacles
DebugData debug_data;
const bool is_driving_forward = motion_utils::isDrivingForward(msg->points);
const bool is_driving_forward = motion_utils::isDrivingForwardWithTwist(msg->points);
const auto target_obstacles = getTargetObstacles(
*msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, is_driving_forward,
debug_data);
Expand Down

0 comments on commit 57ce89a

Please sign in to comment.