Merge pull request #1481 from tier4/feature/multi-level-lanelet-support

Feature/multi level lanelet support

.github/workflows/custom_spell.json

@@ -46,6 +46,8 @@
     "Tschirnhaus",
     "walltime",
     "xerces",
-    "xercesc"
+    "xercesc",
+    "Szymon",
+    "Parapura"
   ]
 }

common/math/geometry/include/geometry/plane.hpp

@@ -0,0 +1,45 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// 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 GEOMETRY__PLANE_HPP_
+#define GEOMETRY__PLANE_HPP_
+
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <optional>
+
+namespace math
+{
+namespace geometry
+{
+
+/// @class Plane
+/// @brief Represents a plane in 3D space, defined by a normal vector and a point on the plane.
+///
+/// The plane is described using the equation:
+///     Ax + By + Cz + D = 0
+/// where:
+/// - A, B, C are the components of the normal vector (normal_ attribute).
+/// - D is the offset from the origin, calculated using the point and normal vector (d_ attribute).
+struct Plane
+{
+  Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal);
+  auto offset(const geometry_msgs::msg::Point & point) const -> double;
+
+  const geometry_msgs::msg::Vector3 normal_;
+  const double d_;
+};
+}  // namespace geometry
+}  // namespace math
+#endif  // GEOMETRY__PLANE_HPP_

common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp

@@ -0,0 +1,39 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// 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 GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_
+#define GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_
+
+#include <Eigen/Geometry>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>>, std::nullptr_t> = nullptr>
+auto getAngleDifference(const T & quat1, const T & quat2) -> double
+{
+  const Eigen::Quaterniond q1(quat1.w, quat1.x, quat1.y, quat1.z);
+  const Eigen::Quaterniond q2(quat2.w, quat2.x, quat2.y, quat2.z);
+
+  const Eigen::AngleAxisd delta(q1.inverse() * q2);
+
+  return std::abs(delta.angle());  // [rad]
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_

common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp

@@ -0,0 +1,41 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// 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 GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_
+#define GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_
+
+#include <geometry/quaternion/get_rotation_matrix.hpp>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>>, std::nullptr_t> = nullptr>
+auto getNormalVector(const T & orientation) -> geometry_msgs::msg::Vector3
+{
+  const Eigen::Matrix3d rotation_matrix = getRotationMatrix(orientation);
+
+  return geometry_msgs::build<geometry_msgs::msg::Vector3>()
+    .x(rotation_matrix(0, 2))
+    .y(rotation_matrix(1, 2))
+    .z(rotation_matrix(2, 2));
+}
+
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_

common/math/geometry/src/plane.cpp

@@ -0,0 +1,39 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// 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.
+
+#include <cmath>
+#include <geometry/plane.hpp>
+#include <geometry/quaternion/operator.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace math
+{
+namespace geometry
+{
+Plane::Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal)
+: normal_(normal), d_(-(normal.x * point.x + normal.y * point.y + normal.z * point.z))
+{
+  if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) {
+    THROW_SIMULATION_ERROR("Plane cannot be created using zero normal vector.");
+  } else if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) {
+    THROW_SIMULATION_ERROR("Plane cannot be created using point with NaN value.");
+  }
+}
+
+auto Plane::offset(const geometry_msgs::msg::Point & point) const -> double
+{
+  return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_;
+}
+}  // namespace geometry
+}  // namespace math

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -139,6 +139,8 @@ class ActionNode : public BT::ActionNodeBase
     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const
     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
+  auto isOtherEntityAtConsideredAltitude(
+    const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool;
 };
 }  // namespace entity_behavior
 

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -316,7 +316,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon(
   double width_extension_left, double length_extension_front, double length_extension_rear) const
   -> std::optional<double>
 {
-  if (status.laneMatchingSucceed()) {
+  if (status.laneMatchingSucceed() && isOtherEntityAtConsideredAltitude(status)) {
     const auto polygon = math::geometry::transformPoints(
       status.getMapPose(), math::geometry::getPointsFromBbox(
                              status.getBoundingBox(), width_extension_right, width_extension_left,
@@ -326,6 +326,13 @@ auto ActionNode::getDistanceToTargetEntityPolygon(
   return std::nullopt;
 }
 
+auto ActionNode::isOtherEntityAtConsideredAltitude(
+  const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool
+{
+  return hdmap_utils->isAltitudeMatching(
+    canonicalized_entity_status->getAltitude(), entity_status.getAltitude());
+}
+
 auto ActionNode::getDistanceToConflictingEntity(
   const lanelet::Ids & route_lanelets,
   const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>

simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp

@@ -17,7 +17,10 @@
 
 #include <simulation_api_schema.pb.h>
 
+#include <geometry/plane.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 #include <memory>
+#include <optional>
 #include <queue>
 #include <random>
 #include <rclcpp/rclcpp.hpp>
@@ -48,13 +51,20 @@ class DetectionSensorBase
     const std::vector<traffic_simulator_msgs::EntityStatus> &) const
     -> std::vector<traffic_simulator_msgs::EntityStatus>::const_iterator;
 
+  auto isOnOrAboveEgoPlane(
+    const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool;
+
 public:
   virtual ~DetectionSensorBase() = default;
 
   virtual void update(
     const double current_simulation_time, const std::vector<traffic_simulator_msgs::EntityStatus> &,
     const rclcpp::Time & current_ros_time,
     const std::vector<std::string> & lidar_detected_entities) = 0;
+
+private:
+  std::optional<math::geometry::Plane> ego_plane_opt_{std::nullopt};
+  std::optional<geometry_msgs::msg::Pose> ego_plane_pose_opt_{std::nullopt};
 };
 
 template <typename T, typename U = autoware_perception_msgs::msg::TrackedObjects>

simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp

@@ -19,6 +19,8 @@
 #include <boost/uuid/uuid.hpp>
 #include <boost/uuid/uuid_generators.hpp>
 #include <boost/uuid/uuid_io.hpp>
+#include <geometry/quaternion/get_angle_difference.hpp>
+#include <geometry/quaternion/get_normal_vector.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <geometry/vector3/hypot.hpp>
 #include <memory>
@@ -62,6 +64,51 @@ auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached(
   }
 }
 
+auto DetectionSensorBase::isOnOrAboveEgoPlane(
+  const geometry_msgs::Pose & entity_pose, const geometry_msgs::Pose & ego_pose) -> bool
+{
+  /*
+      The threshold for detecting significant changes in ego vehicle's orientation (unit: radian).
+      The value determines the minimum angular difference required to consider the ego orientation
+      as "changed".
+
+      There is no technical basis for this value, it was determined based on experiments.
+  */
+  constexpr static double rotation_threshold_ = 0.04;
+  /*
+      Maximum downward offset in Z-axis relative to the ego position (unit: meter).
+      If the NPC is lower than this offset relative to the ego position,
+      the NPC will be excluded from detection
+
+      There is no technical basis for this value, it was determined based on experiments.
+  */
+  constexpr static double max_downward_z_offset_ = 1.0;
+
+  const auto hasEgoOrientationChanged = [this](const geometry_msgs::msg::Pose & ego_pose_ros) {
+    return math::geometry::getAngleDifference(
+             ego_pose_ros.orientation, ego_plane_pose_opt_->orientation) > rotation_threshold_;
+  };
+
+  // if other entity is at the same altitude as Ego or within max_downward_z_offset_ below Ego
+  if (entity_pose.position().z() >= (ego_pose.position().z() - max_downward_z_offset_)) {
+    return true;
+    // otherwise check if other entity is above ego plane
+  } else {
+    // update ego plane if needed
+    geometry_msgs::msg::Pose ego_pose_ros;
+    simulation_interface::toMsg(ego_pose, ego_pose_ros);
+    if (!ego_plane_opt_ || !ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) {
+      ego_plane_opt_.emplace(
+        ego_pose_ros.position, math::geometry::getNormalVector(ego_pose_ros.orientation));
+      ego_plane_pose_opt_ = ego_pose_ros;
+    }
+
+    geometry_msgs::msg::Pose entity_pose_ros;
+    simulation_interface::toMsg(entity_pose, entity_pose_ros);
+    return ego_plane_opt_->offset(entity_pose_ros.position) >= 0.0;
+  }
+}
+
 template <typename To, typename... From>
 auto make(From &&...) -> To;
 
@@ -321,6 +368,7 @@ auto DetectionSensor<autoware_perception_msgs::msg::DetectedObjects>::update(
     auto is_in_range = [&](const auto & status) {
       return not isEgoEntityStatusToWhichThisSensorIsAttached(status) and
              distance(status.pose(), ego_entity_status->pose()) <= configuration_.range() and
+             isOnOrAboveEgoPlane(status.pose(), ego_entity_status->pose()) and
              (configuration_.detect_all_objects_in_range() or
               std::find(
                 lidar_detected_entities.begin(), lidar_detected_entities.end(), status.name()) !=

simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp

@@ -54,6 +54,8 @@ class CanonicalizedEntityStatus
   auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &;
   auto setMapPose(const geometry_msgs::msg::Pose & pose) -> void;
 
+  auto getAltitude() const -> double;
+
   auto getTwist() const noexcept -> const geometry_msgs::msg::Twist &;
   auto setTwist(const geometry_msgs::msg::Twist & twist) -> void;
   auto setLinearVelocity(double linear_velocity) -> void;

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -160,7 +160,7 @@ class HdMapUtils
     const traffic_simulator::RoutingGraphType type =
       traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids;
 
-  auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double;
+  auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double;
 
   auto getLaneChangeTrajectory(
     const geometry_msgs::msg::Pose & from,
@@ -380,6 +380,13 @@ class HdMapUtils
   auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = true)
     const -> geometry_msgs::msg::PoseStamped;
 
+  auto isAltitudeMatching(const double current_altitude, const double target_altitude) const
+    -> bool;
+
+  auto getLaneletAltitude(
+    const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose,
+    const double matching_distance = 1.0) const -> std::optional<double>;
+
 private:
   /** @defgroup cache
    *  Declared mutable for caching

simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp

@@ -117,7 +117,8 @@ class TrafficSource : public TrafficModuleBase
   const std::size_t id;
 
 private:
-  auto makeRandomPose(const bool random_orientation = false) -> geometry_msgs::msg::Pose;
+  auto makeRandomPose(const bool random_orientation, const VehicleOrPedestrianParameter & parameter)
+    -> geometry_msgs::msg::Pose;
 
   auto isPoseValid(const VehicleOrPedestrianParameter &, const geometry_msgs::msg::Pose &)
     -> std::pair<bool, std::optional<CanonicalizedLaneletPose>>;

simulation/traffic_simulator/src/data_type/entity_status.cpp

@@ -122,6 +122,11 @@ auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_ms
   return entity_status_.pose;
 }
 
+auto CanonicalizedEntityStatus::getAltitude() const -> double
+{
+  return entity_status_.pose.position.z;
+}
+
 auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose &
 {
   if (canonicalized_lanelet_pose_) {