forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 30
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(behavior_path_planner): external request lane change (autowarefo…
…undation#2442) * feature(behavior_path_planner): add external request lane change module Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feature(behavior_path_planner): fix for RTC Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feature(behavior_path_planner): fix decision logic Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): fix behavior_path_planner_tree.xml Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): fix for rebase Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): output multiple candidate paths Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): get path candidate in behavior tree manager Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): fix for multiple candidate path Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): separate external request lane change module Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feature(behavior_path_planner): add create publisher method Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feature(behavior_path_planner): move publishers to node Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feature(behavior_path_planner): remove unnecessary publisher Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): move reset path candidate function to behavior tree manager Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): add external request lane change path candidate publisher Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> feat(behavior_path_planner): apply abort lane change Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_path_planner): remove unnecessary change Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feat(behavior_path_planner): fix getLaneChangePaths() Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feat(behavior_path_planner): disable external request lane change in default tree Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * Update rtc_auto_mode_manager.param.yaml * fix(route_handler): remove redundant code * fix(behavior_path_planner): fix for turn signal Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
- Loading branch information
Showing
18 changed files
with
1,164 additions
and
62 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
74 changes: 74 additions & 0 deletions
74
...ning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,74 @@ | ||
| <?xml version="1.0"?> | ||
| <root main_tree_to_execute="BehaviorTree"> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="BehaviorTree"> | ||
| <Fallback> | ||
| <ReactiveSequence> | ||
| <Condition ID="PullOver_Request"/> | ||
| <Action ID="PullOver_Plan" output="{output}"/> | ||
| </ReactiveSequence> | ||
| <ReactiveSequence> | ||
| <Condition ID="PullOut_Request"/> | ||
| <Action ID="PullOut_Plan" output="{output}"/> | ||
| </ReactiveSequence> | ||
| <ReactiveSequence> | ||
| <Condition ID="SideShift_Request"/> | ||
| <Action ID="SideShift_Plan" output="{output}"/> | ||
| </ReactiveSequence> | ||
| <ReactiveSequence> | ||
| <Condition ID="ExternalRequestLaneChangeRight_Request"/> | ||
| <Action ID="ExternalRequestLaneChangeRight_Plan" output="{output}"/> | ||
| </ReactiveSequence> | ||
| <ReactiveSequence> | ||
| <Condition ID="ExternalRequestLaneChangeLeft_Request"/> | ||
| <Action ID="ExternalRequestLaneChangeLeft_Plan" output="{output}"/> | ||
| </ReactiveSequence> | ||
| <ReactiveSequence> | ||
| <Condition ID="LaneChange_Request"/> | ||
| <Action ID="LaneChange_Plan" output="{output}"/> | ||
| </ReactiveSequence> | ||
| <ReactiveSequence> | ||
| <Condition ID="Avoidance_Request"/> | ||
| <Action ID="Avoidance_Plan" output="{output}"/> | ||
| </ReactiveSequence> | ||
| <Action ID="LaneFollowing_Plan" output="{output}"/> | ||
| </Fallback> | ||
| </BehaviorTree> | ||
| <!-- ////////// --> | ||
| <TreeNodesModel> | ||
| <Action ID="Avoidance_Plan"> | ||
| <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port> | ||
| </Action> | ||
| <Condition ID="Avoidance_Request"/> | ||
| <Condition ID="ExternalApproval"/> | ||
| <Action ID="ExternalRequestLaneChangeRight_Plan"> | ||
| <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port> | ||
| </Action> | ||
| <Condition ID="ExternalRequestLaneChangeRight_Request"/> | ||
| <Action ID="ExternalRequestLaneChangeLeft_Plan"> | ||
| <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port> | ||
| </Action> | ||
| <Condition ID="ExternalRequestLaneChangeLeft_Request"/> | ||
| <Action ID="LaneChange_Plan"> | ||
| <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port> | ||
| </Action> | ||
| <Condition ID="LaneChange_Request"/> | ||
| <Action ID="LaneFollowing_Plan"> | ||
| <output_port name="output" type="boost::optional<tier4_planning_msgs::PathWithLaneId_<std::allocator<void> > >">desc</output_port> | ||
| </Action> | ||
| <Condition ID="LaneFollowing_Request"/> | ||
| <Action ID="PullOut_Plan"> | ||
| <output_port name="output"/> | ||
| </Action> | ||
| <Condition ID="PullOut_Request"/> | ||
| <Action ID="PullOver_Plan"> | ||
| <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port> | ||
| </Action> | ||
| <Condition ID="PullOver_Request"/> | ||
| <Action ID="SideShift_Plan"> | ||
| <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port> | ||
| </Action> | ||
| <Condition ID="SideShift_Request"/> | ||
| </TreeNodesModel> | ||
| <!-- ////////// --> | ||
| </root> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
158 changes: 158 additions & 0 deletions
158
...de/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,158 @@ | ||
| // Copyright 2022 TIER IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ | ||
| #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ | ||
|
|
||
| #include "behavior_path_planner/scene_module/lane_change/debug.hpp" | ||
| #include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" | ||
| #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" | ||
| #include "behavior_path_planner/scene_module/scene_module_interface.hpp" | ||
| #include "behavior_path_planner/turn_signal_decider.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" | ||
| #include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" | ||
| #include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" | ||
| #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp> | ||
| #include <geometry_msgs/msg/pose.hpp> | ||
| #include <geometry_msgs/msg/twist.hpp> | ||
|
|
||
| #include <tf2/utils.h> | ||
|
|
||
| #include <memory> | ||
| #include <string> | ||
| #include <unordered_map> | ||
| #include <utility> | ||
| #include <vector> | ||
|
|
||
| namespace behavior_path_planner | ||
| { | ||
| using autoware_auto_planning_msgs::msg::PathWithLaneId; | ||
| using geometry_msgs::msg::Pose; | ||
| using geometry_msgs::msg::Twist; | ||
| using marker_utils::CollisionCheckDebug; | ||
| using tier4_planning_msgs::msg::LaneChangeDebugMsg; | ||
| using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; | ||
|
|
||
| class ExternalRequestLaneChangeModule : public SceneModuleInterface | ||
| { | ||
| public: | ||
| enum class Direction { | ||
| RIGHT = 0, | ||
| LEFT, | ||
| }; | ||
|
|
||
| ExternalRequestLaneChangeModule( | ||
| const std::string & name, rclcpp::Node & node, std::shared_ptr<LaneChangeParameters> parameters, | ||
| const Direction & direction); | ||
|
|
||
| bool isExecutionRequested() const override; | ||
| bool isExecutionReady() const override; | ||
| BT::NodeStatus updateState() override; | ||
| BehaviorModuleOutput plan() override; | ||
| BehaviorModuleOutput planWaitingApproval() override; | ||
| CandidateOutput planCandidate() const override; | ||
| void onEntry() override; | ||
| void onExit() override; | ||
|
|
||
| std::shared_ptr<LaneChangeDebugMsgArray> get_debug_msg_array() const; | ||
| void acceptVisitor( | ||
| [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override; | ||
|
|
||
| protected: | ||
| std::shared_ptr<LaneChangeParameters> parameters_; | ||
| LaneChangeStatus status_; | ||
| PathShifter path_shifter_; | ||
| mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; | ||
| LaneChangeStates current_lane_change_state_; | ||
| std::shared_ptr<LaneChangePath> abort_path_; | ||
| PathWithLaneId prev_approved_path_; | ||
|
|
||
| Direction direction_; | ||
|
|
||
| double lane_change_lane_length_{200.0}; | ||
| double check_distance_{100.0}; | ||
| bool is_abort_path_approved_{false}; | ||
| bool is_abort_approval_requested_{false}; | ||
| bool is_abort_condition_satisfied_{false}; | ||
| bool is_activated_ = false; | ||
|
|
||
| void resetParameters(); | ||
| void resetPathIfAbort(); | ||
|
|
||
| void waitApproval(const double start_distance, const double finish_distance) | ||
| { | ||
| updateRTCStatus(start_distance, finish_distance); | ||
| is_waiting_approval_ = true; | ||
| } | ||
|
|
||
| lanelet::ConstLanelets get_original_lanes() const; | ||
| PathWithLaneId getReferencePath() const; | ||
| lanelet::ConstLanelets getLaneChangeLanes( | ||
| const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; | ||
| std::pair<bool, bool> getSafePath( | ||
| const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, | ||
| LaneChangePath & safe_path) const; | ||
|
|
||
| void updateLaneChangeStatus(); | ||
| void generateExtendedDrivableArea(PathWithLaneId & path); | ||
| void updateOutputTurnSignal(BehaviorModuleOutput & output); | ||
| void updateSteeringFactorPtr(const BehaviorModuleOutput & output); | ||
| void updateSteeringFactorPtr( | ||
| const CandidateOutput & output, const LaneChangePath & selected_path) const; | ||
| void calcTurnSignalInfo(); | ||
|
|
||
| bool isSafe() const; | ||
| bool isValidPath(const PathWithLaneId & path) const; | ||
| bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; | ||
| bool isNearEndOfLane() const; | ||
| bool isCurrentSpeedLow() const; | ||
| bool isAbortConditionSatisfied(); | ||
| bool hasFinishedLaneChange() const; | ||
| bool isAbortState() const; | ||
|
|
||
| // getter | ||
| Pose getEgoPose() const; | ||
| Twist getEgoTwist() const; | ||
| std_msgs::msg::Header getRouteHeader() const; | ||
|
|
||
| // debug | ||
| mutable std::unordered_map<std::string, CollisionCheckDebug> object_debug_; | ||
| mutable std::vector<LaneChangePath> debug_valid_path_; | ||
|
|
||
| void setObjectDebugVisualization() const; | ||
| }; | ||
|
|
||
| class ExternalRequestLaneChangeRightModule : public ExternalRequestLaneChangeModule | ||
| { | ||
| public: | ||
| ExternalRequestLaneChangeRightModule( | ||
| const std::string & name, rclcpp::Node & node, | ||
| std::shared_ptr<LaneChangeParameters> parameters); | ||
| }; | ||
|
|
||
| class ExternalRequestLaneChangeLeftModule : public ExternalRequestLaneChangeModule | ||
| { | ||
| public: | ||
| ExternalRequestLaneChangeLeftModule( | ||
| const std::string & name, rclcpp::Node & node, | ||
| std::shared_ptr<LaneChangeParameters> parameters); | ||
| }; | ||
|
|
||
| } // namespace behavior_path_planner | ||
| // clang-format off | ||
| #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ // NOLINT | ||
| // clang-format on |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.