fix(follow_trajectory_action): adapt to work with considering slopes

simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -64,6 +64,10 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     }
   };
 
+  auto getMatchingDistance = [&]() -> double {
+    return entity_status->getBoundingBox().dimensions.y * 0.5 + 1.0;
+  };
+
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
@@ -76,7 +80,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
   } else if (
     const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*entity_status), *polyline_trajectory,
-      behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) {
+      behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) {
     setOutput(
       "updated_status",
       std::make_shared<traffic_simulator::CanonicalizedEntityStatus>(

simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -64,6 +64,14 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     }
   };
 
+  auto getMatchingDistance = [&]() -> double {
+    return std::max(
+             vehicle_parameters.axles.front_axle.track_width,
+             vehicle_parameters.axles.rear_axle.track_width) *
+             0.5 +
+           1.0;
+  };
+
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
@@ -76,7 +84,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
   } else if (
     const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*entity_status), *polyline_trajectory,
-      behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) {
+      behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) {
     setOutput(
       "updated_status",
       std::make_shared<traffic_simulator::CanonicalizedEntityStatus>(

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp

@@ -62,6 +62,8 @@ class EgoEntitySimulation
 
   const bool consider_pose_by_road_slope_;
 
+  Eigen::Vector3d world_relative_position_;
+
 public:
   const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
 

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -215,20 +215,23 @@ void EgoEntitySimulation::overwrite(
   const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time,
   double step_time, bool npc_logic_started)
 {
+  using quaternion_operation::convertQuaternionToEulerAngle;
+  using quaternion_operation::getRotationMatrix;
+
   autoware->rethrow();
 
-  if (npc_logic_started) {
-    using quaternion_operation::convertQuaternionToEulerAngle;
-    using quaternion_operation::getRotationMatrix;
-
-    auto world_relative_position = [&]() -> Eigen::VectorXd {
-      auto v = Eigen::VectorXd(3);
-      v(0) = status.pose.position.x - initial_pose_.position.x;
-      v(1) = status.pose.position.y - initial_pose_.position.y;
-      v(2) = status.pose.position.z - initial_pose_.position.z;
-      return getRotationMatrix(initial_pose_.orientation).transpose() * v;
-    }();
+  /*
+     SimModelInterface only supports 2D, therefore the position in Oz is
+     considered unchangeable and stored in an additional variable
+     world_relative_position_ that is used in calculations.
+  */
+  world_relative_position_ = getRotationMatrix(initial_pose_.orientation).transpose() *
+                             Eigen::Vector3d(
+                               status.pose.position.x - initial_pose_.position.x,
+                               status.pose.position.y - initial_pose_.position.y,
+                               status.pose.position.z - initial_pose_.position.z);
 
+  if (npc_logic_started) {
     const auto yaw = [&]() {
       const auto q = Eigen::Quaterniond(
         getRotationMatrix(initial_pose_.orientation).transpose() *
@@ -259,8 +262,8 @@ void EgoEntitySimulation::overwrite(
         [[fallthrough]];
 
       case VehicleModelType::IDEAL_STEER_VEL:
-        state(0) = world_relative_position(0);
-        state(1) = world_relative_position(1);
+        state(0) = world_relative_position_.x();
+        state(1) = world_relative_position_.y();
         state(2) = yaw;
         vehicle_model_ptr_->setState(state);
         break;
@@ -277,8 +280,21 @@ void EgoEntitySimulation::overwrite(
 void EgoEntitySimulation::update(
   double current_scenario_time, double step_time, bool npc_logic_started)
 {
+  using quaternion_operation::getRotationMatrix;
+
   autoware->rethrow();
 
+  /*
+     SimModelInterface only supports 2D, therefore the position in Oz is
+     considered unchangeable and stored in an additional variable
+     world_relative_position_ that is used in calculations.
+  */
+  world_relative_position_ = getRotationMatrix(initial_pose_.orientation).transpose() *
+                             Eigen::Vector3d(
+                               status_.pose.position.x - initial_pose_.position.x,
+                               status_.pose.position.y - initial_pose_.position.y,
+                               status_.pose.position.z - initial_pose_.position.z);
+
   if (npc_logic_started) {
     auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU());
 
@@ -319,6 +335,9 @@ void EgoEntitySimulation::update(
     vehicle_model_ptr_->setInput(input);
     vehicle_model_ptr_->update(step_time);
   }
+  // only the position in the Oz axis is left unchanged, the rest is taken from SimModelInterface
+  world_relative_position_.x() = vehicle_model_ptr_->getX();
+  world_relative_position_.y() = vehicle_model_ptr_->getY();
   updateStatus(current_scenario_time, step_time);
   updatePreviousValues();
 }
@@ -331,19 +350,23 @@ auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus(
   /// EgoEntity::setMapPose
   const auto unique_route_lanelets =
     traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets());
-  const auto matching_length = [entity_width] { return entity_width * 0.5 + 1.0; }();
+  const auto matching_distance = std::max(
+                                   vehicle_parameters.axles.front_axle.track_width,
+                                   vehicle_parameters.axles.rear_axle.track_width) *
+                                   0.5 +
+                                 1.0;
 
   std::optional<traffic_simulator_msgs::msg::LaneletPose> lanelet_pose;
 
   if (unique_route_lanelets.empty()) {
     lanelet_pose =
-      hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_length);
+      hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_distance);
   } else {
     lanelet_pose =
-      hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, matching_length);
+      hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, matching_distance);
     if (!lanelet_pose) {
       lanelet_pose =
-        hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_length);
+        hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_distance);
     }
   }
   return lanelet_pose;
@@ -363,33 +386,31 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double
   auto centerline_points = hdmap_utils_ptr_->getCenterPoints(ego_lanelet.value().lanelet_id);
 
   /// @note Copied from motion_util::findNearestSegmentIndex
-  auto find_nearest_segment_index = [](
-                                      const std::vector<geometry_msgs::msg::Point> & points,
-                                      const geometry_msgs::msg::Point & point) {
-    assert(not points.empty());
-
-    double min_dist = std::numeric_limits<double>::max();
-    size_t min_idx = 0;
-
-    for (size_t i = 0; i < points.size(); ++i) {
-      const auto dist = [](const auto point1, const auto point2) {
-        const auto dx = point1.x - point2.x;
-        const auto dy = point1.y - point2.y;
-        return dx * dx + dy * dy;
-      }(points.at(i), point);
-
-      if (dist < min_dist) {
-        min_dist = dist;
-        min_idx = i;
+  auto find_nearest_segment_index =
+    [](const std::vector<geometry_msgs::msg::Point> & points, const Eigen::Vector3d & point) {
+      assert(not points.empty());
+
+      double min_dist = std::numeric_limits<double>::max();
+      size_t min_idx = 0;
+
+      for (size_t i = 0; i < points.size(); ++i) {
+        const auto dist =
+          [](const geometry_msgs::msg::Point & point1, const Eigen::Vector3d & point2) {
+            const auto dx = point1.x - point2.x();
+            const auto dy = point1.y - point2.y();
+            return dx * dx + dy * dy;
+          }(points.at(i), point);
+
+        if (dist < min_dist) {
+          min_dist = dist;
+          min_idx = i;
+        }
       }
-    }
-    return min_idx;
-  };
+      return min_idx;
+    };
 
-  geometry_msgs::msg::Point ego_point;
-  ego_point.x = vehicle_model_ptr_->getX();
-  ego_point.y = vehicle_model_ptr_->getY();
-  const size_t ego_seg_idx = find_nearest_segment_index(centerline_points, ego_point);
+  const size_t ego_seg_idx =
+    find_nearest_segment_index(centerline_points, world_relative_position_);
 
   const auto & prev_point = centerline_points.at(ego_seg_idx);
   const auto & next_point = centerline_points.at(ego_seg_idx + 1);
@@ -419,26 +440,20 @@ auto EgoEntitySimulation::getCurrentTwist() const -> geometry_msgs::msg::Twist
 auto EgoEntitySimulation::getCurrentPose(const double pitch_angle = 0.) const
   -> geometry_msgs::msg::Pose
 {
-  Eigen::VectorXd relative_position(3);
-  relative_position(0) = vehicle_model_ptr_->getX();
-  relative_position(1) = vehicle_model_ptr_->getY();
-  relative_position(2) = 0.0;
-  relative_position =
-    quaternion_operation::getRotationMatrix(initial_pose_.orientation) * relative_position;
-
-  geometry_msgs::msg::Pose current_pose;
-  current_pose.position.x = initial_pose_.position.x + relative_position(0);
-  current_pose.position.y = initial_pose_.position.y + relative_position(1);
-  current_pose.position.z = initial_pose_.position.z + relative_position(2);
-  current_pose.orientation = [&]() {
-    geometry_msgs::msg::Vector3 rpy;
-    rpy.x = 0;
-    rpy.y = pitch_angle;
-    rpy.z = vehicle_model_ptr_->getYaw();
-    return initial_pose_.orientation * quaternion_operation::convertEulerAngleToQuaternion(rpy);
-  }();
-
-  return current_pose;
+  const auto relative_position =
+    quaternion_operation::getRotationMatrix(initial_pose_.orientation) * world_relative_position_;
+  const auto relative_orientation = quaternion_operation::convertEulerAngleToQuaternion(
+    geometry_msgs::build<geometry_msgs::msg::Vector3>()
+      .x(0)
+      .y(pitch_angle)
+      .z(vehicle_model_ptr_->getYaw()));
+
+  return geometry_msgs::build<geometry_msgs::msg::Pose>()
+    .position(geometry_msgs::build<geometry_msgs::msg::Point>()
+                .x(initial_pose_.position.x + relative_position(0))
+                .y(initial_pose_.position.y + relative_position(1))
+                .z(initial_pose_.position.z + relative_position(2)))
+    .orientation(initial_pose_.orientation * relative_orientation);
 }
 
 auto EgoEntitySimulation::getCurrentAccel(const double step_time) const -> geometry_msgs::msg::Accel

simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp

@@ -29,7 +29,7 @@ auto makeUpdatedStatus(
   const traffic_simulator_msgs::msg::EntityStatus &,
   traffic_simulator_msgs::msg::PolylineTrajectory &,
   const traffic_simulator_msgs::msg::BehaviorParameter &,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> &, double,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> &, double, double,
   std::optional<double> target_speed = std::nullopt)
   -> std::optional<traffic_simulator_msgs::msg::EntityStatus>;
 }  // namespace follow_trajectory

simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp

@@ -110,7 +110,7 @@ class FollowWaypointController
      There is no technical basis for this value, it was determined based on
      Dawid Moszynski experiments.
   */
-  static constexpr double finish_distance_tolerance = 1e-4;
+  static constexpr double finish_distance_tolerance = 1e-2;
 
   /*
      Accuracy of the predicted arrival distance at the waypoint with the
@@ -129,7 +129,7 @@ class FollowWaypointController
      There is no technical basis for this value, it was determined based on
      Dawid Moszynski experiments.
   */
-  static constexpr std::size_t number_of_acceleration_candidates = 20;
+  static constexpr std::size_t number_of_acceleration_candidates = 30;
 
   /*
      This is a debugging method, it is not worth giving it much attention.

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -212,7 +212,7 @@ class EntityBase
 
   /*   */ auto setLinearJerk(const double liner_jerk) -> void;
 
-  virtual void startNpcLogic();
+  virtual void startNpcLogic(const double current_time);
 
   /*   */ void stopAtCurrentPosition();
 

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -528,7 +528,7 @@ class EntityManager
       // FIXME: this ignores V2I traffic lights
       iter->second->setTrafficLightManager(conventional_traffic_light_manager_ptr_);
       if (npc_logic_started_ && not is<EgoEntity>(name)) {
-        iter->second->startNpcLogic();
+        iter->second->startNpcLogic(getCurrentTime());
       }
       return success;
     } else {
@@ -555,7 +555,7 @@ class EntityManager
 
   void updateHdmapMarker();
 
-  void startNpcLogic();
+  void startNpcLogic(const double current_time);
 
   auto isNpcLogicStarted() const { return npc_logic_started_; }
 };

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

@@ -284,6 +284,11 @@ class HdMapUtils
     const double matching_distance = 1.0) const
     -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
 
+  auto toLaneletPose(
+    const geometry_msgs::msg::Point &, const traffic_simulator_msgs::msg::BoundingBox &,
+    const bool include_crosswalk, const double matching_distance = 1.0) const
+    -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
+
   auto toLaneletPose(
     const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &,
     const bool include_crosswalk, const double matching_distance = 1.0) const

simulation/traffic_simulator/src/api/api.cpp

@@ -338,8 +338,8 @@ void API::startNpcLogic()
   if (isNpcLogicStarted()) {
     THROW_SIMULATION_ERROR("NPC logics are already started.");
   }
-  entity_manager_ptr_->startNpcLogic();
   clock_.start();
+  entity_manager_ptr_->startNpcLogic(getCurrentTime());
 }
 
 void API::requestLaneChange(const std::string & name, const lanelet::Id & lanelet_id)