Skip to content

Commit

Permalink
feat(behavior_path_planner): add rtc_interface to behavior_path_plann…
Browse files Browse the repository at this point in the history 
…er (#997)

* fix(behavior_path_planner): add rtc_interface (including some bugs)

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feature(behavior_path_planner): remove approval_handler

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feature(behavior_path_planner): apply RTC interface

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): fix removeRTCStatus

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): fix exit status

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feature(behavior_path_planner): add candidate output struct

* feature(behavior_path_planner): add rtc interface to pull_out

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feature(behavior_path_planner): add rtc_interface to pull_over

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feature(behavior_path_planner): remove approval handler

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feature(behavior_path_planner): remove unused functions

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feature(behavior_path_planner): move methods to manage RTC to interface class

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* ci(pre-commit): autofix

* fix(behavior_path_planner): fix nullptr check

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): move removeRTCStatus to private function

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): fix behavior tree config file

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): remove unused variable

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): fix interface

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@rej55 @pre-commit-ci
rej55 and pre-commit-ci[bot] authored Jun 16, 2022
1 parent 737ab3c commit e40754a
Show file tree
Hide file tree
Showing 25 changed files with 560 additions and 290 deletions.
16 changes: 0 additions & 16 deletions planning/behavior_path_planner/config/behavior_path_planner_tree.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,30 +5,22 @@
<Fallback>
<ReactiveSequence>
<Condition ID="PullOver_Request"/>
<Condition ID="PullOver_Ready"/>
<Action ID="PullOver_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="PullOut_Request"/>
<Condition ID="PullOut_Ready"/>
<Action ID="PullOut_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="SideShift_Request"/>
<Condition ID="SideShift_Ready"/>
<Action ID="SideShift_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="LaneChange_Request"/>
<Fallback>
<Condition ID="LaneChange_Ready"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
</Fallback>
<Action ID="LaneChange_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="Avoidance_Request"/>
<Condition ID="Avoidance_Ready"/>
<Action ID="Avoidance_Plan" output="{output}"/>
</ReactiveSequence>
<Action ID="LaneFollowing_Plan" output="{output}"/>
Expand Down Expand Up @@ -61,37 +53,30 @@
<Action ID="Avoidance_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="Avoidance_Ready"/>
<Condition ID="Avoidance_Request"/>
<Condition ID="ExternalApproval"/>
<Condition ID="ForceApproval"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
<Condition ID="LaneChange_CheckApproval"/>
<Action ID="LaneChange_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="LaneChange_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="LaneChange_Ready"/>
<Condition ID="LaneChange_Request"/>
<Action ID="LaneFollowing_Plan">
<output_port name="output" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Action ID="LaneFollowing_PlanCandidate">
<output_port name="output_candidate" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Condition ID="LaneFollowing_Ready"/>
<Condition ID="LaneFollowing_Request"/>
<Action ID="PullOut_Plan">
<output_port name="output"/>
</Action>
<Condition ID="PullOut_Ready"/>
<Condition ID="PullOut_Request"/>
<Action ID="PullOver_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="PullOver_Ready"/>
<Condition ID="PullOver_Request"/>
<Condition ID="SideShift_CheckApproval"/>
<Action ID="SideShift_Plan">
Expand All @@ -100,7 +85,6 @@
<Action ID="SideShift_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="SideShift_Ready"/>
<Condition ID="SideShift_Request"/>
<SubTree ID="SubTree"/>
<SubTree ID="a"/>
Expand Down
14 changes: 0 additions & 14 deletions planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,20 +5,14 @@
<Fallback>
<ReactiveSequence>
<Condition ID="PullOver_Request"/>
<Condition ID="PullOver_Ready"/>
<Action ID="PullOver_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="PullOut_Request"/>
<Condition ID="PullOut_Ready"/>
<Action ID="PullOut_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="LaneChange_Request"/>
<Fallback>
<Condition ID="LaneChange_Ready"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
</Fallback>
<Action ID="LaneChange_Plan" output="{output}"/>
</ReactiveSequence>
<Action ID="LaneFollowing_Plan" output="{output}"/>
Expand Down Expand Up @@ -51,37 +45,30 @@
<Action ID="Avoidance_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="Avoidance_Ready"/>
<Condition ID="Avoidance_Request"/>
<Condition ID="ExternalApproval"/>
<Condition ID="ForceApproval"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
<Condition ID="LaneChange_CheckApproval"/>
<Action ID="LaneChange_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="LaneChange_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="LaneChange_Ready"/>
<Condition ID="LaneChange_Request"/>
<Action ID="LaneFollowing_Plan">
<output_port name="output" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Action ID="LaneFollowing_PlanCandidate">
<output_port name="output_candidate" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Condition ID="LaneFollowing_Ready"/>
<Condition ID="LaneFollowing_Request"/>
<Action ID="PullOut_Plan">
<output_port name="output"/>
</Action>
<Condition ID="PullOut_Ready"/>
<Condition ID="PullOut_Request"/>
<Action ID="PullOver_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="PullOver_Ready"/>
<Condition ID="PullOver_Request"/>
<Condition ID="SideShift_CheckApproval"/>
<Action ID="SideShift_Plan">
Expand All @@ -90,7 +77,6 @@
<Action ID="SideShift_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="SideShift_Ready"/>
<Condition ID="SideShift_Request"/>
<SubTree ID="SubTree"/>
<SubTree ID="a"/>
Expand Down
100 changes: 100 additions & 0 deletions planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Fallback>
<ReactiveSequence>
<Condition ID="PullOver_Request"/>
<Condition ID="PullOver_Ready"/>
<Action ID="PullOver_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="PullOut_Request"/>
<Condition ID="PullOut_Ready"/>
<Action ID="PullOut_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="LaneChange_Request"/>
<Fallback>
<Condition ID="LaneChange_Ready"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
</Fallback>
<Action ID="LaneChange_Plan" output="{output}"/>
</ReactiveSequence>
<Action ID="LaneFollowing_Plan" output="{output}"/>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="a">
<ReactiveFallback>
<Condition ID="LaneChange_CheckApproval"/>
<KeepRunningUntilFailure>
<Action ID="LaneChange_PlanCandidate" output="{output}"/>
</KeepRunningUntilFailure>
</ReactiveFallback>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="b">
<ReactiveFallback>
<Condition ID="Avoidance_CheckApproval"/>
<KeepRunningUntilFailure>
<Action ID="Avoidance_PlanCandidate" output="{output}"/>
</KeepRunningUntilFailure>
</ReactiveFallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Condition ID="Avoidance_CheckApproval"/>
<Action ID="Avoidance_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="Avoidance_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="Avoidance_Ready"/>
<Condition ID="Avoidance_Request"/>
<Condition ID="ExternalApproval"/>
<Condition ID="ForceApproval"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
<Condition ID="LaneChange_CheckApproval"/>
<Action ID="LaneChange_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="LaneChange_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="LaneChange_Ready"/>
<Condition ID="LaneChange_Request"/>
<Action ID="LaneFollowing_Plan">
<output_port name="output" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Action ID="LaneFollowing_PlanCandidate">
<output_port name="output_candidate" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Condition ID="LaneFollowing_Ready"/>
<Condition ID="LaneFollowing_Request"/>
<Action ID="PullOut_Plan">
<output_port name="output"/>
</Action>
<Condition ID="PullOut_Ready"/>
<Condition ID="PullOut_Request"/>
<Action ID="PullOver_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="PullOver_Ready"/>
<Condition ID="PullOver_Request"/>
<Condition ID="SideShift_CheckApproval"/>
<Action ID="SideShift_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="SideShift_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="SideShift_Ready"/>
<Condition ID="SideShift_Request"/>
<SubTree ID="SubTree"/>
<SubTree ID="a"/>
<SubTree ID="b"/>
</TreeNodesModel>
<!-- ////////// -->
</root>
110 changes: 110 additions & 0 deletions planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Fallback>
<ReactiveSequence>
<Condition ID="PullOver_Request"/>
<Condition ID="PullOver_Ready"/>
<Action ID="PullOver_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="PullOut_Request"/>
<Condition ID="PullOut_Ready"/>
<Action ID="PullOut_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="SideShift_Request"/>
<Condition ID="SideShift_Ready"/>
<Action ID="SideShift_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="LaneChange_Request"/>
<Fallback>
<Condition ID="LaneChange_Ready"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
</Fallback>
<Action ID="LaneChange_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="Avoidance_Request"/>
<Condition ID="Avoidance_Ready"/>
<Action ID="Avoidance_Plan" output="{output}"/>
</ReactiveSequence>
<Action ID="LaneFollowing_Plan" output="{output}"/>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="a">
<ReactiveFallback>
<Condition ID="LaneChange_CheckApproval"/>
<KeepRunningUntilFailure>
<Action ID="LaneChange_PlanCandidate" output="{output}"/>
</KeepRunningUntilFailure>
</ReactiveFallback>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="b">
<ReactiveFallback>
<Condition ID="Avoidance_CheckApproval"/>
<KeepRunningUntilFailure>
<Action ID="Avoidance_PlanCandidate" output="{output}"/>
</KeepRunningUntilFailure>
</ReactiveFallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Condition ID="Avoidance_CheckApproval"/>
<Action ID="Avoidance_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="Avoidance_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="Avoidance_Ready"/>
<Condition ID="Avoidance_Request"/>
<Condition ID="ExternalApproval"/>
<Condition ID="ForceApproval"/>
<Condition ID="ForceLaneChange_ForceApproval"/>
<Condition ID="LaneChange_CheckApproval"/>
<Action ID="LaneChange_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="LaneChange_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="LaneChange_Ready"/>
<Condition ID="LaneChange_Request"/>
<Action ID="LaneFollowing_Plan">
<output_port name="output" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Action ID="LaneFollowing_PlanCandidate">
<output_port name="output_candidate" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
</Action>
<Condition ID="LaneFollowing_Ready"/>
<Condition ID="LaneFollowing_Request"/>
<Action ID="PullOut_Plan">
<output_port name="output"/>
</Action>
<Condition ID="PullOut_Ready"/>
<Condition ID="PullOut_Request"/>
<Action ID="PullOver_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="PullOver_Ready"/>
<Condition ID="PullOver_Request"/>
<Condition ID="SideShift_CheckApproval"/>
<Action ID="SideShift_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Action ID="SideShift_PlanCandidate">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="SideShift_Ready"/>
<Condition ID="SideShift_Request"/>
<SubTree ID="SubTree"/>
<SubTree ID="a"/>
<SubTree ID="b"/>
</TreeNodesModel>
<!-- ////////// -->
</root>
7 changes: 0 additions & 7 deletions planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,13 +159,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
PathWithLaneId::SharedPtr getPathCandidate(
const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);

/**
* @brief publish behavior module status mainly for the user interface
*/
void publishModuleStatus(
const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses,
const std::shared_ptr<PlannerData> planner_data);

// debug

private:
Expand Down
1 change: 0 additions & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@ class BehaviorTreeManager
BehaviorTreeManager(rclcpp::Node & node, const BehaviorTreeManagerParam & param);
void createBehaviorTree();
void registerSceneModule(const std::shared_ptr<SceneModuleInterface> & p);
void registerForceApproval(const std::string & name);

void resetBehaviorTree();

Expand Down
Loading

0 comments on commit e40754a

Please sign in to comment.