feat(tier4_autoware_utils): add insert_target_point from 514fea4

[yn-mrse]

Description

insertTargetPoint関数を、現時点autoware.universe 最新の

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

Lines 968 to 1252 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);
	}
	/**
	* @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;
	}

からコピー。

Related links

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

Lines 968 to 1252 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);
	}
	/**
	* @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;
	}

Tests performed

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

Lines 968 to 1252 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);
	}
	/**
	* @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;
	}

と完全に同一の内容であることを確認済み。

Notes for reviewers

build testをパスするためには、事前に #599 をマージする必要があります。

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.

[yn-mrse]

このPRは #600 へと統合するため削除します。