feat(tier4_autoware_utils): add geometry func

[yn-mrse]

Description

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

• getLongitudinalVelocity
• setPose
• setOrientation
• setLongitudinalVelocity
• isDrivingForward

その依存関係である下記をincludeします。

• tf2/utils.h
  • (isDrivingForward 内でtf2::getYaw関数を利用するため)
• tier4_autoware_utils/math/constants.hpp
  • (isDrivingForward 内で定数piを利用するため)
• tier4_autoware_utils/math/normalization.hpp
  • (isDrivingForward 内でnormarizeRadian関数を利用するため)

また、現時点autoware.universe最新のhttps://github.com/tier4/autoware.universe/blob/f321e40c02328f9d7ad6ec7c6a210e1a53e6b369/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp をコピーします。

Related links

• autoware.universe/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

  Lines 186 to 267 in f321e40

  	template <class T>
  	double getLongitudinalVelocity([[maybe_unused]] const T & p)
  	{
  	static_assert(sizeof(T) == 0, "Only specializations of getVelocity can be used.");
  	throw std::logic_error("Only specializations of getVelocity can be used.");
  	}
  	template <>
  	inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::PathPoint & p)
  	{
  	return p.longitudinal_velocity_mps;
  	}
  	template <>
  	inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p)
  	{
  	return p.longitudinal_velocity_mps;
  	}
  	template <class T>
  	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.");
  	}
  	template <>
  	inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & p)
  	{
  	p = pose;
  	}
  	template <>
  	inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::PoseStamped & p)
  	{
  	p.pose = pose;
  	}
  	template <>
  	inline void setPose(
  	const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPoint & p)
  	{
  	p.pose = pose;
  	}
  	template <>
  	inline void setPose(
  	const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p)
  	{
  	p.pose = pose;
  	}
  	template <class T>
  	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.");
  	}
  	template <>
  	inline void setLongitudinalVelocity(
  	const double velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p)
  	{
  	p.longitudinal_velocity_mps = velocity;
  	}
  	template <>
  	inline void setLongitudinalVelocity(
  	const double velocity, autoware_auto_planning_msgs::msg::PathPoint & p)
  	{
  	p.longitudinal_velocity_mps = velocity;
  	}

• autoware.universe/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

  Lines 624 to 633 in f321e40

  	template <class Pose1, class Pose2>
  	bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose)
  	{
  	// check the first point direction
  	const double src_yaw = tf2::getYaw(getPose(src_pose).orientation);
  	const double pose_direction_yaw = calcAzimuthAngle(getPoint(src_pose), getPoint(dst_pose));
  	return std::fabs(normalizeRadian(src_yaw - pose_direction_yaw)) < pi / 2.0;
  	}

• https://github.com/tier4/autoware.universe/blob/f321e40c02328f9d7ad6ec7c6a210e1a53e6b369/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp

Tests performed

上記リンクと完全に同一の内容であることを確認済みです。
build testは#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.

[codecov-commenter]

Codecov Report

Patch coverage has no change and project coverage change: -0.01 ⚠️

Comparison is base (0a72a37) 10.01% compared to head (464b381) 10.01%.

❗ Your organization is not using the GitHub App Integration. As a result you may experience degraded service beginning May 15th. Please install the Github App Integration for your organization. Read more.

Additional details and impacted files

@@                         Coverage Diff                         @@
##           feat/keep_stopping_dist/v0.3.16     #599      +/-   ##
===================================================================
- Coverage                            10.01%   10.01%   -0.01%     
===================================================================
  Files                                  427      427              
  Lines                                35924    35925       +1     
  Branches                              8090     8091       +1     
===================================================================
  Hits                                  3597     3597              
  Misses                               28879    28879              
- Partials                              3448     3449       +1     

Flag	Coverage Δ
differential	10.01% <ø> (-0.01%)	⬇️

Impacted Files	Coverage Δ
...include/tier4_autoware_utils/geometry/geometry.hpp	91.02% <ø> (ø)

... and 1 file with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.