feat(tier4_autoware_utils): add trajectory func

[yn-mrse]

Description

下記の関数を現時点autoware.universe 最新の https://github.com/tier4/autoware.universe/blob/514fea4bf80bf460fa880d96377eb1230823fb7b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp からコピーします。

• validateNonSharpAngle
• isDrivingForward
• isDrivingForwardWithTwist
• insertTargetPoint

また、現時点autoware.universe最新の https://github.com/tier4/autoware.universe/blob/7ef1bee35c60cfc4637840bfacc0162d14e31143/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp をコピーし、tier4_autoware_utilsとして置き換えます。

Related links

• autoware.universe/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

  Lines 45 to 117 in 514fea4

  	/**
  	* @brief validate a point is in a non-sharp angle between two points or not
  	* @param point1 front point
  	* @param point2 point to be validated
  	* @param point3 back point
  	*/
  	template <class T>
  	void validateNonSharpAngle(
  	const T & point1, const T & point2, const T & point3,
  	const double angle_threshold = tier4_autoware_utils::pi / 4)
  	{
  	const auto p1 = tier4_autoware_utils::getPoint(point1);
  	const auto p2 = tier4_autoware_utils::getPoint(point2);
  	const auto p3 = tier4_autoware_utils::getPoint(point3);
  	const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z};
  	const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z};
  	const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0);
  	const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2);
  	const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2);
  	constexpr double epsilon = 1e-3;
  	if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) {
  	throw std::invalid_argument("Sharp angle.");
  	}
  	}
  	/**
  	* @brief checks whether a path of trajectory has forward driving direction
  	* @param points points of trajectory, path, ...
  	* @return (forward / backward) driving (true / false)
  	*/
  	template <class T>
  	boost::optional<bool> isDrivingForward(const T points)
  	{
  	if (points.size() < 2) {
  	return boost::none;
  	}
  	// check the first point direction
  	const auto & first_pose = tier4_autoware_utils::getPose(points.at(0));
  	const auto & second_pose = tier4_autoware_utils::getPose(points.at(1));
  	return tier4_autoware_utils::isDrivingForward(first_pose, second_pose);
  	}
  	/**
  	* @brief checks whether a path of trajectory has forward driving direction using its longitudinal
  	* velocity
  	* @param points_with_twist points of trajectory, path, ... (with velocity)
  	* @return (forward / backward) driving (true, false, none "if velocity is zero")
  	*/
  	template <class T>
  	boost::optional<bool> isDrivingForwardWithTwist(const T points_with_twist)
  	{
  	if (points_with_twist.empty()) {
  	return boost::none;
  	}
  	if (points_with_twist.size() == 1) {
  	if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
  	return true;
  	} else if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
  	return false;
  	} else {
  	return boost::none;
  	}
  	}
  	return isDrivingForward(points_with_twist);
  	}

• autoware.universe/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

  Lines 968 to 1186 in 514fea4

  	/**
  	* @brief insert a point in points container (trajectory, path, ...) using segment id
  	* @param seg_idx segment index of point at beginning of length
  	* @param p_target point to be inserted
  	* @param points output points of trajectory, path, ...
  	* @param overlap_threshold distance threshold, used to check if the inserted point is between start
  	* and end of nominated segment to be added in.
  	* @return index of segment id, where point is inserted
  	*/
  	template <class T>
  	inline boost::optional<size_t> insertTargetPoint(
  	const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points,
  	const double overlap_threshold = 1e-3)
  	{
  	try {
  	validateNonEmpty(points);
  	} catch (const std::exception & e) {
  	std::cerr << e.what() << std::endl;
  	return {};
  	}
  	// invalid segment index
  	if (seg_idx + 1 >= points.size()) {
  	return {};
  	}
  	const auto p_front = tier4_autoware_utils::getPoint(points.at(seg_idx));
  	const auto p_back = tier4_autoware_utils::getPoint(points.at(seg_idx + 1));
  	try {
  	validateNonSharpAngle(p_front, p_target, p_back);
  	} catch (const std::exception & e) {
  	std::cerr << e.what() << std::endl;
  	return {};
  	}
  	const auto overlap_with_front =
  	tier4_autoware_utils::calcDistance2d(p_target, p_front) < overlap_threshold;
  	const auto overlap_with_back =
  	tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold;
  	const auto is_driving_forward = isDrivingForward(points);
  	if (!is_driving_forward) {
  	return {};
  	}
  	geometry_msgs::msg::Pose target_pose;
  	{
  	const auto p_base = is_driving_forward.get() ? 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);
  	}
  	auto p_insert = points.at(seg_idx);
  	tier4_autoware_utils::setPose(target_pose, p_insert);
  	geometry_msgs::msg::Pose base_pose;
  	{
  	const auto p_base = is_driving_forward.get() ? 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);
  	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) {
  	if (is_driving_forward.get()) {
  	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;
  	}
  	if (overlap_with_back) {
  	return seg_idx + 1;
  	}
  	return seg_idx;
  	}
  	/**
  	* @brief insert a point in points container (trajectory, path, ...) using length of point to be
  	* inserted
  	* @param insert_point_length length to insert point from the beginning of the points
  	* @param p_target point to be inserted
  	* @param points output points of trajectory, path, ...
  	* @param overlap_threshold distance threshold, used to check if the inserted point is between start
  	* and end of nominated segment to be added in.
  	* @return index of segment id, where point is inserted
  	*/
  	template <class T>
  	inline boost::optional<size_t> insertTargetPoint(
  	const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points,
  	const double overlap_threshold = 1e-3)
  	{
  	validateNonEmpty(points);
  	if (insert_point_length < 0.0) {
  	return boost::none;
  	}
  	// Get Nearest segment index
  	boost::optional<size_t> segment_idx = boost::none;
  	for (size_t i = 1; i < points.size(); ++i) {
  	const double length = calcSignedArcLength(points, 0, i);
  	if (insert_point_length <= length) {
  	segment_idx = i - 1;
  	break;
  	}
  	}
  	if (!segment_idx) {
  	return boost::none;
  	}
  	return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold);
  	}
  	/**
  	* @brief insert a point in points container (trajectory, path, ...) using segment index and length
  	* of point to be inserted
  	* @param src_segment_idx source segment index on the trajectory
  	* @param insert_point_length length to insert point from the beginning of the points
  	* @param points output points of trajectory, path, ...
  	* @param overlap_threshold distance threshold, used to check if the inserted point is between start
  	* and end of nominated segment to be added in.
  	* @return index of insert point
  	*/
  	template <class T>
  	inline boost::optional<size_t> insertTargetPoint(
  	const size_t src_segment_idx, const double insert_point_length, T & points,
  	const double overlap_threshold = 1e-3)
  	{
  	validateNonEmpty(points);
  	if (src_segment_idx >= points.size() - 1) {
  	return boost::none;
  	}
  	// Get Nearest segment index
  	boost::optional<size_t> segment_idx = boost::none;
  	if (0.0 <= insert_point_length) {
  	for (size_t i = src_segment_idx + 1; i < points.size(); ++i) {
  	const double length = calcSignedArcLength(points, src_segment_idx, i);
  	if (insert_point_length <= length) {
  	segment_idx = i - 1;
  	break;
  	}
  	}
  	} else {
  	for (int i = src_segment_idx - 1; 0 <= i; --i) {
  	const double length = calcSignedArcLength(points, src_segment_idx, i);
  	if (length <= insert_point_length) {
  	segment_idx = i;
  	break;
  	}
  	}
  	}
  	if (!segment_idx) {
  	return boost::none;
  	}
  	// Get Target Point
  	const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1);
  	const double target_length =
  	insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx);
  	const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0);
  	const auto p_target = tier4_autoware_utils::calcInterpolatedPoint(
  	tier4_autoware_utils::getPoint(points.at(*segment_idx)),
  	tier4_autoware_utils::getPoint(points.at(*segment_idx + 1)), ratio);
  	return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold);
  	}
  	/**
  	* @brief Insert a target point from a source pose on the trajectory
  	* @param src_pose source pose on the trajectory
  	* @param insert_point_length length to insert point from the beginning of the points
  	* @param points output points of trajectory, path, ...
  	* @param max_dist max distance, used to search for nearest segment index in points container to the
  	* given source pose
  	* @param max_yaw max yaw, used to search for nearest segment index in points container to the given
  	* source pose
  	* @param overlap_threshold distance threshold, used to check if the inserted point is between start
  	* and end of nominated segment to be added in.
  	* @return index of insert point
  	*/
  	template <class T>
  	inline boost::optional<size_t> insertTargetPoint(
  	const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points,
  	const double max_dist = std::numeric_limits<double>::max(),
  	const double max_yaw = std::numeric_limits<double>::max(), const double overlap_threshold = 1e-3)
  	{
  	validateNonEmpty(points);
  	if (insert_point_length < 0.0) {
  	return boost::none;
  	}
  	const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw);
  	if (!nearest_segment_idx) {
  	return boost::none;
  	}
  	const double offset_length =
  	calcLongitudinalOffsetToSegment(points, *nearest_segment_idx, src_pose.position);
  	return insertTargetPoint(
  	*nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold);
  	}

• https://github.com/tier4/autoware.universe/blob/7ef1bee35c60cfc4637840bfacc0162d14e31143/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp

Tests performed

• trajectory.hpp内の関数は、上記リンク箇所の実装と完全に同一内容であることを確認済みです。
• path_with_lane_id.hpp内実装は、上記リンク箇所の実装と同一であり、"motion_utils"だけが"tier4_autoware_utils"へと変更になっていることを確認済みです。
• build testはfeat(behavior_velocity_planner): changeable maximum stop distance (#1… #596 にてまとめて確認済みです。

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[sfukuta]

Related linksにある

autoware.universe/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

Lines 1187 to 1252 in 514fea4

	/**
	* @brief Insert stop point from the source segment index
	* @param src_segment_idx start segment index on the trajectory
	* @param distance_to_stop_point distance to stop point from the source index
	* @param points_with_twist output points of trajectory, path, ... (with velocity)
	* @param overlap_threshold distance threshold, used to check if the inserted point is between start
	* and end of nominated segment to be added in.
	* @return index of stop point
	*/
	template <class T>
	inline boost::optional<size_t> insertStopPoint(
	const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist,
	const double overlap_threshold = 1e-3)
	{
	validateNonEmpty(points_with_twist);
	if (distance_to_stop_point < 0.0 || src_segment_idx >= points_with_twist.size() - 1) {
	return boost::none;
	}
	const auto stop_idx = insertTargetPoint(
	src_segment_idx, distance_to_stop_point, points_with_twist, overlap_threshold);
	if (!stop_idx) {
	return boost::none;
	}
	for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) {
	tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i));
	}
	return stop_idx;
	}
	/**
	* @brief Insert stop point from the front point of the path
	* @param distance_to_stop_point distance to stop point from the front point of the path
	* @param points_with_twist output points of trajectory, path, ... (with velocity)
	* @param overlap_threshold distance threshold, used to check if the inserted point is between start
	* and end of nominated segment to be added in.
	* @return index of stop point
	*/
	template <class T>
	inline boost::optional<size_t> insertStopPoint(
	const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3)
	{
	validateNonEmpty(points_with_twist);
	if (distance_to_stop_point < 0.0) {
	return boost::none;
	}
	double accumulated_length = 0;
	for (size_t i = 0; i < points_with_twist.size() - 1; ++i) {
	const auto curr_pose = tier4_autoware_utils::getPose(points_with_twist.at(i));
	const auto next_pose = tier4_autoware_utils::getPose(points_with_twist.at(i + 1));
	const double length = tier4_autoware_utils::calcDistance2d(curr_pose, next_pose);
	if (accumulated_length + length + overlap_threshold > distance_to_stop_point) {
	const double insert_length = distance_to_stop_point - accumulated_length;
	return insertStopPoint(i, insert_length, points_with_twist, overlap_threshold);
	}
	accumulated_length += length;
	}
	return boost::none;
	}

の"insertStopPoint"が修正内容に存在していないです。
Descriptionともあっていないので、どちらか修正をお願いします。

[yn-mrse]

@sfukuta Descriptionを修正しました。