Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/default matching distance #1194

Merged
merged 17 commits into from
Feb 22, 2024
Merged

Feature/default matching distance #1194

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
ade14bc
add getDefaultMatchinDistance function
hakuturu583 Feb 19, 2024
d03c2c2
Merge branch 'doc/lane_pose_calculation' of https://github.com/tier4/…
hakuturu583 Feb 19, 2024
2e9c300
add getDefaultMatchingDistanceForLaneletPoseCalculation() for vehicle…
hakuturu583 Feb 19, 2024
1869a27
fix copmile error
hakuturu583 Feb 19, 2024
72a2355
update docs
hakuturu583 Feb 19, 2024
14d4e0a
fix document
hakuturu583 Feb 19, 2024
d677203
passing matching distance for lanelet_pose_caluculation
hakuturu583 Feb 19, 2024
39169fc
simplify code
hakuturu583 Feb 19, 2024
2e82078
fix mathing algorithum
hakuturu583 Feb 21, 2024
e687bec
Merge https://github.com/tier4/scenario_simulator_v2 into feature/def…
hakuturu583 Feb 21, 2024
586ad81
Merge branch 'feature/default_matching_distance' of https://github.co…
hakuturu583 Feb 21, 2024
b5e3f66
Merge branch 'master' into feature/default_matching_distance
hakuturu583 Feb 22, 2024
59032e1
Update simulation/traffic_simulator/include/traffic_simulator/entity/…
hakuturu583 Feb 22, 2024
d0bfdf6
Merge https://github.com/tier4/scenario_simulator_v2 into feature/def…
hakuturu583 Feb 22, 2024
a2c04ee
Merge branch 'feature/default_matching_distance' of https://github.co…
hakuturu583 Feb 22, 2024
17b5d94
update document
hakuturu583 Feb 22, 2024
e02b39c
fix typo
hakuturu583 Feb 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
12 changes: 11 additions & 1 deletion docs/developer_guide/lane_pose_calculation/Spawn.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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$$
8 changes: 4 additions & 4 deletions docs/developer_guide/lane_pose_calculation/UpdateFrame.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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$$
Expand Down Expand Up @@ -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).
Expand Down
6 changes: 4 additions & 2 deletions simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"),
Expand Down Expand Up @@ -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;
Expand Down
43 changes: 22 additions & 21 deletions simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
41 changes: 21 additions & 20 deletions simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
7 changes: 7 additions & 0 deletions simulation/behavior_tree_plugin/src/action_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}
Expand Down
12 changes: 7 additions & 5 deletions simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
/**
Expand Down
Loading
Loading