Merge pull request #1194 from tier4/feature/default_matching_distance

Feature/default matching distance

docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md

@@ -19,12 +19,12 @@ These are the [timing immediately after the spawn of the Entity](Spawn.md), the
 
 The unit of the table is meter.
 
-| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md)                                                          | [Getting longitudinal distance](GetLongitudinalDistance.md) |
-| ---------------------------------- | ----------------- | -------------------------------------------------------------------------------------- | ----------------------------------------------------------- |
-| EgoEntity                          | 2.0               | [(tread of the entity) + 2.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) | 10                                                          |
-| VehicleEntity                      | 2.0               | N/A                                                                                    | 10                                                          |
-| PedestrianEntity                   | 2.0               | [2.0 or 4.0](UpdateFrame.md#pedestrian-entity-with-behavior-tree)                      | 10                                                          |
-| MiscObject                         | 2.0               | N/A                                                                                    | 10                                                          |
+| EntityType \ Timing of calculation | [Spawn](Spawn.md)                                                                             | [UpdateFrame](UpdateFrame.md)                                                          | [Getting longitudinal distance](GetLongitudinalDistance.md) |
+| ---------------------------------- | --------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | ----------------------------------------------------------- |
+| EgoEntity                          | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system)              | [(tread of the entity) + 2.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) | 10                                                          |
+| VehicleEntity                      | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system)              | N/A                                                                                    | 10                                                          |
+| PedestrianEntity                   | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | [2.0 or 4.0](UpdateFrame.md#pedestrian-entity-with-behavior-tree)                      | 10                                                          |
+| MiscObject                         | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | N/A                                                                                    | 10                                                          |
 
 ### Detail of the lane coordinate system calculation algorithm for a specific lane
 

docs/developer_guide/lane_pose_calculation/Spawn.md

@@ -30,4 +30,14 @@ After narrowing down the candidate lanes by the above procedure, the pose of the
 
 ![Lane pose calculation](../../image/lane_pose_calculation.png "Lane pose calculation.")
 
-In this case, the length of the horizontal bar is `2.0`m.
+### Vehicle / Ego Entity
+
+Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be $t_f$ and the tread of the rear wheels be $t_r$.
+
+$$L_m = max(t_r, t_f) + 2.0$$
+
+### Pedestrian / MiscObject Entity
+
+Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and width of the bounding box be $L_w$
+
+$$L_m = L_w + 2.0$$

docs/developer_guide/lane_pose_calculation/UpdateFrame.md

@@ -18,7 +18,7 @@ If 3 is executed and fails, fallbacks to 4.
 
 ### Calculate pose in lane coordinate system
 
-Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be  $t_f$ and the tread of the rear wheels be $t_r$.
+Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be $t_f$ and the tread of the rear wheels be $t_r$.
 See also [here.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L278-L284)
 
 $$L_m = max(t_r, t_f) + 2.0$$
@@ -47,11 +47,11 @@ The procedure for calculating the pose in the lane coordinate system at this tim
 
 If lane matching was successful in the previous frame, do 1, otherwise do 2
 
-1.[Set the length of the horizontal bar to 2.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c 27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L72-L77)  
-2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0 c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L79-L83)  
+1.[Set the length of the horizontal bar to 2.0 + the width of the bounding box and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L72-L77)  
+2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L79-L85)  
 
 If calculation 1 or 2 fails,  
-3.[Set the length of the horizontal bar to 4.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c 27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L86-L91)  
+3.[Set the length of the horizontal bar to 4.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L86-L91)  
 If 1 or 2 are successful, then 3 is skipped.
 
 If the pose could not be calculated in the lane coordinate system by considering up to the result of 3, [the pose calculation in the lane coordinate system is a failure](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L125).

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -78,15 +78,16 @@ class ActionNode : public BT::ActionNodeBase
     return {
       // clang-format off
       BT::InputPort<double>("current_time"),
+      BT::InputPort<double>("matching_distance_for_lanelet_pose_calculation"),
       BT::InputPort<double>("step_time"),
       BT::InputPort<EntityStatusDict>("other_entity_status"),
+      BT::InputPort<lanelet::Ids>("route_lanelets"),
       BT::InputPort<std::optional<double>>("target_speed"),
       BT::InputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils"),
       BT::InputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>("entity_status"),
+      BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightManager>>("traffic_light_manager"),
       BT::InputPort<std::unordered_map<std::string, traffic_simulator_msgs::msg::EntityType>>("entity_type_list"),
-      BT::InputPort<lanelet::Ids>("route_lanelets"),
       BT::InputPort<traffic_simulator::behavior::Request>("request"),
-      BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightManager>>("traffic_light_manager"),
       BT::OutputPort<std::optional<traffic_simulator_msgs::msg::Obstacle>>("obstacle"),
       BT::OutputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>("updated_status"),
       BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>("waypoints"),
@@ -116,6 +117,7 @@ class ActionNode : public BT::ActionNodeBase
   std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> entity_status;
   double current_time;
   double step_time;
+  double default_matching_distance_for_lanelet_pose_calculation;
   std::optional<double> target_speed;
   std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> updated_status;
   EntityStatusDict other_entity_status;

simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp

@@ -49,27 +49,28 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
   }
 
   // clang-format off
-  DEFINE_GETTER_SETTER(BehaviorParameter,    traffic_simulator_msgs::msg::BehaviorParameter)
-  DEFINE_GETTER_SETTER(CurrentTime,          double)
-  DEFINE_GETTER_SETTER(DebugMarker,          std::vector<visualization_msgs::msg::Marker>)
-  DEFINE_GETTER_SETTER(EntityTypeList,       EntityTypeDict)
-  DEFINE_GETTER_SETTER(GoalPoses,            std::vector<geometry_msgs::msg::Pose>)
-  DEFINE_GETTER_SETTER(EntityStatus,         std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
-  DEFINE_GETTER_SETTER(PolylineTrajectory,   std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
-  DEFINE_GETTER_SETTER(HdMapUtils,           std::shared_ptr<hdmap_utils::HdMapUtils>)
-  DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter)
-  DEFINE_GETTER_SETTER(Obstacle,             std::optional<traffic_simulator_msgs::msg::Obstacle>)
-  DEFINE_GETTER_SETTER(OtherEntityStatus,    EntityStatusDict)
-  DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters)
-  DEFINE_GETTER_SETTER(ReferenceTrajectory,  std::shared_ptr<math::geometry::CatmullRomSpline>)
-  DEFINE_GETTER_SETTER(Request,              traffic_simulator::behavior::Request)
-  DEFINE_GETTER_SETTER(RouteLanelets,        lanelet::Ids)
-  DEFINE_GETTER_SETTER(StepTime,             double)
-  DEFINE_GETTER_SETTER(TargetSpeed,          std::optional<double>)
-  DEFINE_GETTER_SETTER(TrafficLightManager,  std::shared_ptr<traffic_simulator::TrafficLightManager>)
-  DEFINE_GETTER_SETTER(UpdatedStatus,        std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
-  DEFINE_GETTER_SETTER(VehicleParameters,    traffic_simulator_msgs::msg::VehicleParameters)
-  DEFINE_GETTER_SETTER(Waypoints,            traffic_simulator_msgs::msg::WaypointsArray)
+  DEFINE_GETTER_SETTER(BehaviorParameter,                                traffic_simulator_msgs::msg::BehaviorParameter)
+  DEFINE_GETTER_SETTER(CurrentTime,                                      double)
+  DEFINE_GETTER_SETTER(DebugMarker,                                      std::vector<visualization_msgs::msg::Marker>)
+  DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double)
+  DEFINE_GETTER_SETTER(EntityTypeList,                                   EntityTypeDict)
+  DEFINE_GETTER_SETTER(GoalPoses,                                        std::vector<geometry_msgs::msg::Pose>)
+  DEFINE_GETTER_SETTER(EntityStatus,                                     std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
+  DEFINE_GETTER_SETTER(PolylineTrajectory,                               std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
+  DEFINE_GETTER_SETTER(HdMapUtils,                                       std::shared_ptr<hdmap_utils::HdMapUtils>)
+  DEFINE_GETTER_SETTER(LaneChangeParameters,                             traffic_simulator::lane_change::Parameter)
+  DEFINE_GETTER_SETTER(Obstacle,                                         std::optional<traffic_simulator_msgs::msg::Obstacle>)
+  DEFINE_GETTER_SETTER(OtherEntityStatus,                                EntityStatusDict)
+  DEFINE_GETTER_SETTER(PedestrianParameters,                             traffic_simulator_msgs::msg::PedestrianParameters)
+  DEFINE_GETTER_SETTER(ReferenceTrajectory,                              std::shared_ptr<math::geometry::CatmullRomSpline>)
+  DEFINE_GETTER_SETTER(Request,                                          traffic_simulator::behavior::Request)
+  DEFINE_GETTER_SETTER(RouteLanelets,                                    lanelet::Ids)
+  DEFINE_GETTER_SETTER(StepTime,                                         double)
+  DEFINE_GETTER_SETTER(TargetSpeed,                                      std::optional<double>)
+  DEFINE_GETTER_SETTER(TrafficLightManager,                              std::shared_ptr<traffic_simulator::TrafficLightManager>)
+  DEFINE_GETTER_SETTER(UpdatedStatus,                                    std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
+  DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
+  DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
   // clang-format on
 
 #undef DEFINE_GETTER_SETTER

simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp

@@ -55,26 +55,27 @@ class VehicleBehaviorTree : public BehaviorPluginBase
   }
 
   // clang-format off
-  DEFINE_GETTER_SETTER(CurrentTime,          double)
-  DEFINE_GETTER_SETTER(DebugMarker,          std::vector<visualization_msgs::msg::Marker>)
-  DEFINE_GETTER_SETTER(EntityTypeList,       EntityTypeDict)
-  DEFINE_GETTER_SETTER(GoalPoses,            std::vector<geometry_msgs::msg::Pose>)
-  DEFINE_GETTER_SETTER(EntityStatus,         std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
-  DEFINE_GETTER_SETTER(PolylineTrajectory,   std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
-  DEFINE_GETTER_SETTER(HdMapUtils,           std::shared_ptr<hdmap_utils::HdMapUtils>)
-  DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter)
-  DEFINE_GETTER_SETTER(Obstacle,             std::optional<traffic_simulator_msgs::msg::Obstacle>)
-  DEFINE_GETTER_SETTER(OtherEntityStatus,    EntityStatusDict)
-  DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters)
-  DEFINE_GETTER_SETTER(ReferenceTrajectory,  std::shared_ptr<math::geometry::CatmullRomSpline>)
-  DEFINE_GETTER_SETTER(Request,              traffic_simulator::behavior::Request)
-  DEFINE_GETTER_SETTER(RouteLanelets,        lanelet::Ids)
-  DEFINE_GETTER_SETTER(StepTime,             double)
-  DEFINE_GETTER_SETTER(TargetSpeed,          std::optional<double>)
-  DEFINE_GETTER_SETTER(TrafficLightManager,  std::shared_ptr<traffic_simulator::TrafficLightManager>)
-  DEFINE_GETTER_SETTER(UpdatedStatus,        std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
-  DEFINE_GETTER_SETTER(VehicleParameters,    traffic_simulator_msgs::msg::VehicleParameters)
-  DEFINE_GETTER_SETTER(Waypoints,            traffic_simulator_msgs::msg::WaypointsArray)
+  DEFINE_GETTER_SETTER(CurrentTime,                                      double)
+  DEFINE_GETTER_SETTER(DebugMarker,                                      std::vector<visualization_msgs::msg::Marker>)
+  DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double)
+  DEFINE_GETTER_SETTER(EntityStatus,                                     std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
+  DEFINE_GETTER_SETTER(EntityTypeList,                                   EntityTypeDict)
+  DEFINE_GETTER_SETTER(GoalPoses,                                        std::vector<geometry_msgs::msg::Pose>)
+  DEFINE_GETTER_SETTER(HdMapUtils,                                       std::shared_ptr<hdmap_utils::HdMapUtils>)
+  DEFINE_GETTER_SETTER(LaneChangeParameters,                             traffic_simulator::lane_change::Parameter)
+  DEFINE_GETTER_SETTER(Obstacle,                                         std::optional<traffic_simulator_msgs::msg::Obstacle>)
+  DEFINE_GETTER_SETTER(OtherEntityStatus,                                EntityStatusDict)
+  DEFINE_GETTER_SETTER(PedestrianParameters,                             traffic_simulator_msgs::msg::PedestrianParameters)
+  DEFINE_GETTER_SETTER(PolylineTrajectory,                               std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
+  DEFINE_GETTER_SETTER(ReferenceTrajectory,                              std::shared_ptr<math::geometry::CatmullRomSpline>)
+  DEFINE_GETTER_SETTER(Request,                                          traffic_simulator::behavior::Request)
+  DEFINE_GETTER_SETTER(RouteLanelets,                                    lanelet::Ids)
+  DEFINE_GETTER_SETTER(StepTime,                                         double)
+  DEFINE_GETTER_SETTER(TargetSpeed,                                      std::optional<double>)
+  DEFINE_GETTER_SETTER(TrafficLightManager,                              std::shared_ptr<traffic_simulator::TrafficLightManager>)
+  DEFINE_GETTER_SETTER(UpdatedStatus,                                    std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
+  DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
+  DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
   // clang-format on
 #undef DEFINE_GETTER_SETTER
 

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -65,6 +65,13 @@ auto ActionNode::getBlackBoardValues() -> void
     target_speed = std::nullopt;
   }
 
+  if (!getInput<double>(
+        "matching_distance_for_lanelet_pose_calculation",
+        default_matching_distance_for_lanelet_pose_calculation)) {
+    THROW_SIMULATION_ERROR(
+      "failed to get input matching_distance_for_lanelet_pose_calculation in ActionNode");
+  }
+
   if (!getInput<EntityStatusDict>("other_entity_status", other_entity_status)) {
     THROW_SIMULATION_ERROR("failed to get input other_entity_status in ActionNode");
   }

simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp

@@ -70,17 +70,19 @@ auto PedestrianActionNode::estimateLaneletPose(const geometry_msgs::msg::Pose &
   std::optional<traffic_simulator::LaneletPose> lanelet_pose;
   if (entity_status->laneMatchingSucceed()) {
     /**
-     * @note Hard coded parameter. 1.0 is a matching threshold for lanelet.
-     * In this branch, try to matching pedestrian entity to specified lanelet_id.
+     * @note In this branch, try to matching pedestrian entity to specified lanelet_id.
     */
-    lanelet_pose =
-      hdmap_utils->toLaneletPose(pose, entity_status->getLaneletPose().lanelet_id, 1.0);
+    lanelet_pose = hdmap_utils->toLaneletPose(
+      pose, entity_status->getLaneletPose().lanelet_id,
+      default_matching_distance_for_lanelet_pose_calculation);
   } else {
     /**
      * @note In this branch, try to matching pedestrian entity considering bounding box.
      * true means considering crosswalk.
     */
-    lanelet_pose = hdmap_utils->toLaneletPose(pose, entity_status->getBoundingBox(), true);
+    lanelet_pose = hdmap_utils->toLaneletPose(
+      pose, entity_status->getBoundingBox(), true,
+      default_matching_distance_for_lanelet_pose_calculation);
   }
   if (!lanelet_pose) {
     /**