feat(tier4_autoware_utils): expand geometry and trajectory utility fu…

…nctions (#990) * feat(tier4_autoware_utils): expand geometry utility functions Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(tier4_autoware_utils): expand trajectory utility functions Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(behavior_path_planner): remove unused function Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): fix typo Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): use calcInterpolatedPoint Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): calcLongitudinalOffsetPose Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): separate path_with_lane_id utils Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): fix interpolated point pose Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): fix copyright Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(behavior_velocity_planner): fix include header file Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Jun 28, 2022 · 940f912 · 940f912
1 parent c286f7c
commit 940f912
Show file tree

Hide file tree

Showing 11 changed files with 2,307 additions and 37 deletions.
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
@@ -114,6 +114,39 @@ inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::
   return p.pose;
 }
 
+template <class T>
+void setPose(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;
+}
+
 inline geometry_msgs::msg::Point createPoint(const double x, const double y, const double z)
 {
   geometry_msgs::msg::Point p;

diff --git a/...tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp b/...tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp
@@ -0,0 +1,46 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_
+#define TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_
+
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
+namespace tier4_autoware_utils
+{
+template <>
+inline geometry_msgs::msg::Point getPoint(
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+{
+  return p.point.pose.position;
+}
+
+template <>
+inline geometry_msgs::msg::Pose getPose(
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+{
+  return p.point.pose;
+}
+
+template <>
+inline void setPose(
+  const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+{
+  p.point.pose = pose;
+}
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp
@@ -17,6 +17,7 @@
 
 #include "tier4_autoware_utils/geometry/boost_geometry.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp"
 #include "tier4_autoware_utils/geometry/pose_deviation.hpp"
 #include "tier4_autoware_utils/math/constants.hpp"
 #include "tier4_autoware_utils/math/normalization.hpp"
@@ -32,6 +33,7 @@
 #include "tier4_autoware_utils/ros/update_param.hpp"
 #include "tier4_autoware_utils/ros/wait_for_param.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
+#include "tier4_autoware_utils/trajectory/path_with_lane_id.hpp"
 #include "tier4_autoware_utils/trajectory/trajectory.hpp"
 #include "tier4_autoware_utils/vehicle/vehicle_state_checker.hpp"
 

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp
@@ -0,0 +1,258 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
+#define TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
+
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp"
+#include "tier4_autoware_utils/trajectory/trajectory.hpp"
+
+#include <boost/optional.hpp>
+
+#include <algorithm>
+#include <limits>
+#include <stdexcept>
+#include <vector>
+
+namespace tier4_autoware_utils
+{
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @param points points of trajectory, path, ...
+ * @param src_idx index of source point
+ * @param offset length of offset from source point
+ * @return offset point
+ */
+template <>
+inline boost::optional<geometry_msgs::msg::Point> calcLongitudinalOffsetPoint(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const size_t src_idx, const double offset)
+{
+  validateNonEmpty(points);
+
+  if (points.size() - 1 < src_idx) {
+    throw std::out_of_range("Invalid source index");
+  }
+
+  if (points.size() == 1) {
+    return {};
+  }
+
+  if (src_idx + 1 == points.size() && offset == 0.0) {
+    return getPoint(points.at(src_idx));
+  }
+
+  if (offset < 0.0) {
+    auto reverse_points = points;
+    std::reverse(reverse_points.begin(), reverse_points.end());
+    return calcLongitudinalOffsetPoint(
+      reverse_points, reverse_points.size() - src_idx - 1, -offset);
+  }
+
+  double dist_sum = 0.0;
+
+  for (size_t i = src_idx; i < points.size() - 1; ++i) {
+    const auto & p_front = points.at(i);
+    const auto & p_back = points.at(i + 1);
+
+    const auto dist_segment = calcDistance2d(p_front, p_back);
+    dist_sum += dist_segment;
+
+    const auto dist_res = offset - dist_sum;
+    if (dist_res <= 0.0) {
+      return calcInterpolatedPoint(p_back, p_front, std::abs(dist_res / dist_segment));
+    }
+  }
+
+  // not found (out of range)
+  return {};
+}
+
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @param points points of trajectory, path, ...
+ * @param src_point source point
+ * @param offset length of offset from source point
+ * @return offset point
+ */
+template <>
+inline boost::optional<geometry_msgs::msg::Point> calcLongitudinalOffsetPoint(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Point & src_point, const double offset)
+{
+  validateNonEmpty(points);
+
+  if (offset < 0.0) {
+    auto reverse_points = points;
+    std::reverse(reverse_points.begin(), reverse_points.end());
+    return calcLongitudinalOffsetPoint(reverse_points, src_point, -offset);
+  }
+
+  const size_t src_seg_idx = findNearestSegmentIndex(points, src_point);
+  const double signed_length_src_offset =
+    calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point);
+
+  return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset);
+}
+
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @param points points of trajectory, path, ...
+ * @param src_idx index of source point
+ * @param offset length of offset from source point
+ * @return offset pose
+ */
+template <>
+inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const size_t src_idx, const double offset)
+{
+  validateNonEmpty(points);
+
+  if (points.size() - 1 < src_idx) {
+    throw std::out_of_range("Invalid source index");
+  }
+
+  if (points.size() == 1) {
+    return {};
+  }
+
+  if (src_idx + 1 == points.size() && offset == 0.0) {
+    return getPose(points.at(src_idx));
+  }
+
+  if (offset < 0.0) {
+    auto reverse_points = points;
+    std::reverse(reverse_points.begin(), reverse_points.end());
+
+    double dist_sum = 0.0;
+
+    for (size_t i = reverse_points.size() - src_idx - 1; i < reverse_points.size() - 1; ++i) {
+      const auto & p_front = reverse_points.at(i);
+      const auto & p_back = reverse_points.at(i + 1);
+
+      const auto dist_segment = calcDistance2d(p_front, p_back);
+      dist_sum += dist_segment;
+
+      const auto dist_res = -offset - dist_sum;
+      if (dist_res <= 0.0) {
+        return calcInterpolatedPose(p_back, p_front, std::abs(dist_res / dist_segment));
+      }
+    }
+  } else {
+    double dist_sum = 0.0;
+
+    for (size_t i = src_idx; i < points.size() - 1; ++i) {
+      const auto & p_front = points.at(i);
+      const auto & p_back = points.at(i + 1);
+
+      const auto dist_segment = calcDistance2d(p_front, p_back);
+      dist_sum += dist_segment;
+
+      const auto dist_res = offset - dist_sum;
+      if (dist_res <= 0.0) {
+        return calcInterpolatedPose(p_front, p_back, 1.0 - std::abs(dist_res / dist_segment));
+      }
+    }
+  }
+
+  // not found (out of range)
+  return {};
+}
+
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @param points points of trajectory, path, ...
+ * @param src_point source point
+ * @param offset length of offset from source point
+ * @return offset pase
+ */
+template <>
+inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Point & src_point, const double offset)
+{
+  validateNonEmpty(points);
+
+  const size_t src_seg_idx = findNearestSegmentIndex(points, src_point);
+  const double signed_length_src_offset =
+    calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point);
+
+  return calcLongitudinalOffsetPose(points, src_seg_idx, offset + signed_length_src_offset);
+}
+
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @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, ...
+ * @return index of insert point
+ */
+template <>
+inline size_t insertTargetPoint(
+  const size_t seg_idx, const geometry_msgs::msg::Point & p_target,
+  std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const double overlap_threshold)
+{
+  validateNonEmpty(points);
+
+  const auto p_front = getPoint(points.at(seg_idx));
+  const auto p_back = 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;
+  }
+
+  const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold;
+  const auto overlap_with_back = calcDistance2d(p_target, p_back) < overlap_threshold;
+
+  geometry_msgs::msg::Pose target_pose;
+  {
+    const auto pitch = calcElevationAngle(p_target, p_back);
+    const auto yaw = calcAzimuthAngle(p_target, p_back);
+
+    target_pose.position = p_target;
+    target_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
+  }
+
+  auto p_insert = points.at(seg_idx);
+  setPose(target_pose, p_insert);
+
+  geometry_msgs::msg::Pose front_pose;
+  {
+    const auto pitch = calcElevationAngle(p_front, p_target);
+    const auto yaw = calcAzimuthAngle(p_front, p_target);
+
+    front_pose.position = getPoint(points.at(seg_idx));
+    front_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
+  }
+
+  if (!overlap_with_front && !overlap_with_back) {
+    setPose(front_pose, points.at(seg_idx));
+    points.insert(points.begin() + seg_idx + 1, p_insert);
+    return seg_idx + 1;
+  }
+
+  if (overlap_with_back) {
+    return seg_idx + 1;
+  }
+
+  return seg_idx;
+}
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_