fix(follow_trajectory_action): adapt to work with considering slopes

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -92,6 +92,7 @@ class ActionNode : public BT::ActionNodeBase
       BT::OutputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>("updated_status"),
       BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>("waypoints"),
       BT::OutputPort<traffic_simulator::behavior::Request>("request"),
+      BT::OutputPort<std::vector<visualization_msgs::msg::Marker>>("debug_marker"),
       // clang-format on
     };
   }

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/vehicle_model/sim_model_delay_steer_acc_geared.hpp

@@ -62,6 +62,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
     VX,
     STEER,
     ACCX,
+    Z,
   };
   enum IDX_U {
     ACCX_DES = 0,
@@ -101,6 +102,11 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
    */
   double getY() override;
 
+  /**
+   * @brief get vehicle position z
+   */
+  double getZ() override;
+
   /**
    * @brief get vehicle angle yaw
    */

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp

@@ -106,6 +106,11 @@ class SimModelInterface
    */
   virtual double getY() = 0;
 
+  /**
+   * @brief get vehicle position z
+   */
+  virtual double getZ() { return 0; };
+
   /**
    * @brief get vehicle angle yaw
    */

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -234,7 +234,6 @@ void EgoEntitySimulation::overwrite(
       return convertQuaternionToEulerAngle(relative_orientation).z -
              (previous_linear_velocity_ ? *previous_angular_velocity_ : 0) * step_time;
     }();
-
     switch (auto state = Eigen::VectorXd(vehicle_model_ptr_->getDimX()); vehicle_model_type_) {
       case VehicleModelType::DELAY_STEER_ACC:
       case VehicleModelType::DELAY_STEER_ACC_GEARED:
@@ -249,6 +248,7 @@ void EgoEntitySimulation::overwrite(
       case VehicleModelType::IDEAL_STEER_ACC:
       case VehicleModelType::IDEAL_STEER_ACC_GEARED:
         state(3) = status.action_status.twist.linear.x;
+        state(6) = world_relative_position(2);
         [[fallthrough]];
 
       case VehicleModelType::IDEAL_STEER_VEL:
@@ -324,19 +324,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;
@@ -415,7 +419,7 @@ auto EgoEntitySimulation::getCurrentPose(const double pitch_angle = 0.) const
   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(2) = vehicle_model_ptr_->getZ();
   relative_position =
     quaternion_operation::getRotationMatrix(initial_pose_.orientation) * relative_position;
 

simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp

@@ -21,7 +21,7 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared(
   double dt, double acc_delay, double acc_time_constant, double steer_delay,
   double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor,
   double debug_steer_scaling_factor)
-: SimModelInterface(6 /* dim x */, 2 /* dim u */),
+: SimModelInterface(7 /* dim x */, 2 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
   vx_rate_lim_(vx_rate_lim),
@@ -41,6 +41,7 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared(
 
 double SimModelDelaySteerAccGeared::getX() { return state_(IDX::X); }
 double SimModelDelaySteerAccGeared::getY() { return state_(IDX::Y); }
+double SimModelDelaySteerAccGeared::getZ() { return state_(IDX::Z); }
 double SimModelDelaySteerAccGeared::getYaw() { return state_(IDX::YAW); }
 double SimModelDelaySteerAccGeared::getVx() { return state_(IDX::VX); }
 double SimModelDelaySteerAccGeared::getVy() { return 0.0; }

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

@@ -231,7 +231,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

@@ -586,7 +586,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 {
@@ -613,7 +613,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

@@ -282,6 +282,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

@@ -285,8 +285,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)