add lateral metrics example (adjust example file)

Signed-off-by: Sarun Mukdapitak <sarun.mukda@gmail.com>

Author: sarun-hub

common/autoware_trajectory/README.md

@@ -297,10 +297,20 @@ common/autoware_trajectory/examples/example_self_intersecting.cpp:352:357
 
 #### <span style="font-size: 1.2em;">`<autoware/trajectory/utils/lateral_metrics.hpp>`</span>
 
-| Function                                     | Description                                                                                                                                                                                   | Detail                                                                                                                                                                                       |
-| -------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| <ul><li>`compute_lateral_distance`</li></ul> | A utility function that computes the normal distance between a given `target point` and the tangent line of the pose at specified arc-length `s` on the trajectory.<br> See below cell figure |                                                                                                                                                                                              |
-| <ul><li>`is_left_side`</li></ul>             | A utility function that determines whether a given `target point` lies on the **left side** of the trajectory's tangent direction at the specified arc-length `s`.                            | ![lateral_metrics](./images/utils/lateral_distance_computation.drawio.svg)[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/lateral_distance_computation.drawio.svg") }}) |
+| Function                                     | Description                                                                                                                                                                                                                        | Detail                                                                                                                                                                                       |
+| -------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| <ul><li>`compute_lateral_distance`</li></ul> | A utility function that computes the normal distance between a given `target point` and the tangent line of the pose at specified arc-length `s` on the trajectory. The result will always be positive. <br> See below cell figure |                                                                                                                                                                                              |
+| <ul><li>`is_left_side`</li></ul>             | A utility function that determines whether a given `target point` lies on the **left side** of the trajectory's tangent direction at the specified arc-length `s`.                                                                 | ![lateral_metrics](./images/utils/lateral_distance_computation.drawio.svg)[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/lateral_distance_computation.drawio.svg") }}) |
+
+##### <span style="font-size: 1.2em;">Example Usage of `lateral_metrics`</span>
+
+Calculate lateral distance from Parabolic Trajectory to given point and check the side of the point from the trajectory.
+
+```cpp title="./examples/example_lateral_metrics.cpp:144:155"
+--8<--
+common/autoware_trajectory/examples/example_lateral_metrics.cpp:144:155
+--8<--
+```
 
 #### <span style="font-size: 1.2em;">`<autoware/trajectory/utils/footprint.hpp>`</span>
 

common/autoware_trajectory/examples/example_lateral_metrics.cpp

@@ -135,13 +135,24 @@ static void draw_line(
   }
 }
 
+template <class TrajectoryPointType>
 static void draw_square_angle_line(
-  autoware::pyplot::Axes & ax, const geometry_msgs::msg::Pose & from_pose,
-  const geometry_msgs::msg::Point & to_point, const std::string & color, const std::string & label)
+  autoware::pyplot::Axes & ax, const Trajectory<TrajectoryPointType> & traj, const double target_s,
+  const geometry_msgs::msg::Pose & from_pose, const geometry_msgs::msg::Point & to_point,
+  const std::string & color, const std::string & label)
 {
-  const auto lateral_offset = autoware_utils_geometry::calc_lateral_deviation(from_pose, to_point);
-  const auto vertical_point =
-    autoware_utils_geometry::calc_offset_pose(from_pose, 0, lateral_offset, 0, 0);
+  // Always give positive value
+  double lateral_offset =
+    autoware::experimental::trajectory::compute_lateral_distance(traj, to_point, target_s);
+  auto on_left = autoware::experimental::trajectory::is_left_side(traj, to_point, target_s);
+
+  geometry_msgs::msg::Pose vertical_point;
+  if (!on_left) {
+    // positive y is translated to left, so need to convert to negative value for right side.
+    vertical_point = autoware_utils_geometry::calc_offset_pose(from_pose, 0, -lateral_offset, 0, 0);
+  } else {  // left
+    vertical_point = autoware_utils_geometry::calc_offset_pose(from_pose, 0, lateral_offset, 0, 0);
+  }
 
   std::vector<double> x_{from_pose.position.x, vertical_point.position.x, to_point.x};
   std::vector<double> y_{from_pose.position.y, vertical_point.position.y, to_point.y};
@@ -195,7 +206,7 @@ int main1()
 
   draw_line(ax1, target_point, vertical_pose.position, "orange", "lateral distance");
   draw_line(ax1, target_point, target_pose.position, "red", "distance2d");
-  draw_square_angle_line(ax1, target_pose, target_point, "black", line_label);
+  draw_square_angle_line(ax1, traj, target_s, target_pose, target_point, "black", line_label);
   draw_tangent_line(ax1, target_pose);
 
   plot_trajectory(ax2, traj, "Parabolic to the right");
@@ -205,7 +216,7 @@ int main1()
 
   draw_line(ax2, target_point, vertical_pose.position, "orange", "lateral distance", true);
   draw_line(ax2, target_point, target_pose.position, "red", "distance2d", true);
-  draw_square_angle_line(ax2, target_pose, target_point, "black", line_label);
+  draw_square_angle_line(ax2, traj, target_s, target_pose, target_point, "black", line_label);
   draw_tangent_line(ax2, target_pose);
 
   ax2.set_title(Args("To the right (Point on the Right)"), Kwargs("fontsize"_a = 16));
@@ -260,7 +271,7 @@ int main2()
   plot_point(ax1, vertical_pose.position, "blue", "Perpendicular Point");
   draw_line(ax1, target_point, vertical_pose.position, "orange", "lateral distance");
   draw_line(ax1, target_point, target_pose.position, "red", "distance2d");
-  draw_square_angle_line(ax1, target_pose, target_point, "black", line_label);
+  draw_square_angle_line(ax1, traj, target_s, target_pose, target_point, "black", line_label);
   draw_tangent_line(ax1, target_pose);
 
   plot_trajectory(ax2, traj, "Parabolic to the right");
@@ -270,7 +281,7 @@ int main2()
 
   draw_line(ax2, target_point, vertical_pose.position, "orange", "lateral distance", true);
   draw_line(ax2, target_point, target_pose.position, "red", "distance2d", true);
-  draw_square_angle_line(ax2, target_pose, target_point, "black", line_label);
+  draw_square_angle_line(ax2, traj, target_s, target_pose, target_point, "black", line_label);
   draw_tangent_line(ax2, target_pose);
 
   ax2.set_title(Args("To the left (Point on the Left)"), Kwargs("fontsize"_a = 16));