From e40754a973b22e4dcf46ea5c20b16d4e84c174bf Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 16 Jun 2022 19:15:26 +0900 Subject: [PATCH] feat(behavior_path_planner): add rtc_interface to behavior_path_planner (#997) * fix(behavior_path_planner): add rtc_interface (including some bugs) Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): remove approval_handler Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): apply RTC interface Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix removeRTCStatus Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix exit status Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): add candidate output struct * feature(behavior_path_planner): add rtc interface to pull_out Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): add rtc_interface to pull_over Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): remove approval handler Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): remove unused functions Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): move methods to manage RTC to interface class Signed-off-by: Fumiya Watanabe * ci(pre-commit): autofix * fix(behavior_path_planner): fix nullptr check Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): move removeRTCStatus to private function Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix behavior tree config file Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): remove unused variable Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix interface Signed-off-by: Fumiya Watanabe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/behavior_path_planner_tree.xml | 16 --- ...ior_path_planner_tree_lane_change_only.xml | 14 --- ...path_planner_tree_lane_change_only_old.xml | 100 ++++++++++++++++ .../config/behavior_path_planner_tree_old.xml | 110 ++++++++++++++++++ .../behavior_path_planner_node.hpp | 7 -- .../behavior_tree_manager.hpp | 1 - .../scene_module/approval_handler.hpp | 67 ----------- .../avoidance/avoidance_module.hpp | 47 +++++++- .../lane_change/lane_change_module.hpp | 61 +++++++++- .../lane_following/lane_following_module.hpp | 3 +- .../scene_module/pull_out/pull_out_module.hpp | 3 +- .../pull_over/pull_over_module.hpp | 3 +- .../scene_module_bt_node_interface.hpp | 5 - .../scene_module/scene_module_interface.hpp | 95 ++++++++++++--- .../side_shift/side_shift_module.hpp | 3 +- planning/behavior_path_planner/package.xml | 1 + .../src/behavior_path_planner_node.cpp | 85 +------------- .../src/behavior_tree_manager.cpp | 17 ++- .../avoidance/avoidance_module.cpp | 48 +++++--- .../lane_change/lane_change_module.cpp | 46 ++++++-- .../lane_following/lane_following_module.cpp | 7 +- .../scene_module/pull_out/pull_out_module.cpp | 40 ++++--- .../pull_over/pull_over_module.cpp | 41 +++++-- .../scene_module_bt_node_interface.cpp | 22 +--- .../side_shift/side_shift_module.cpp | 8 +- 25 files changed, 560 insertions(+), 290 deletions(-) create mode 100644 planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml create mode 100644 planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/approval_handler.hpp diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml index bfae48368da48..748b70b33f181 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml @@ -5,30 +5,22 @@ - - - - - - - - @@ -61,11 +53,8 @@ desc - - - desc @@ -73,7 +62,6 @@ desc - desc @@ -81,17 +69,14 @@ desc - - desc - @@ -100,7 +85,6 @@ desc - diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml index 0a615e48a140c..5c4d2b589d242 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml @@ -5,20 +5,14 @@ - - - - - - @@ -51,11 +45,8 @@ desc - - - desc @@ -63,7 +54,6 @@ desc - desc @@ -71,17 +61,14 @@ desc - - desc - @@ -90,7 +77,6 @@ desc - diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml new file mode 100644 index 0000000000000..0a615e48a140c --- /dev/null +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + desc + + + desc + + + + + + + + + desc + + + desc + + + + + desc + + + desc + + + + + + + + + + desc + + + + + + desc + + + desc + + + + + + + + + diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml new file mode 100644 index 0000000000000..bfae48368da48 --- /dev/null +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + desc + + + desc + + + + + + + + + desc + + + desc + + + + + desc + + + desc + + + + + + + + + + desc + + + + + + desc + + + desc + + + + + + + + + diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 62779c3d5924d..636717be52e9c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -159,13 +159,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node PathWithLaneId::SharedPtr getPathCandidate( const BehaviorModuleOutput & bt_out, const std::shared_ptr planner_data); - /** - * @brief publish behavior module status mainly for the user interface - */ - void publishModuleStatus( - const std::vector> & statuses, - const std::shared_ptr planner_data); - // debug private: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp index 259184d30d722..bcb22abe09ea3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp @@ -48,7 +48,6 @@ class BehaviorTreeManager BehaviorTreeManager(rclcpp::Node & node, const BehaviorTreeManagerParam & param); void createBehaviorTree(); void registerSceneModule(const std::shared_ptr & p); - void registerForceApproval(const std::string & name); void resetBehaviorTree(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/approval_handler.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/approval_handler.hpp deleted file mode 100644 index 457cb5da66e93..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/approval_handler.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2021 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__APPROVAL_HANDLER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__APPROVAL_HANDLER_HPP_ - -#include - -#include -#include - -namespace behavior_path_planner -{ -class ApprovalHandler -{ -public: - explicit ApprovalHandler(rclcpp::Node & node) : clock_{*node.get_clock()} {} - - void setCurrentApproval(const behavior_path_planner::BoolStamped & approval) - { - approval_ = approval; - } - - bool isApproved() const - { - const auto now = clock_.now(); - const auto thresh_sec = 0.5; - if (approval_.data && (now - approval_.stamp).seconds() < thresh_sec) { - if ((now - last_clear_time_).seconds() > thresh_sec) { - return true; - } - } - return false; - } - - bool isWaitingApproval() const { return is_waiting_approval_; } - void waitApproval() { is_waiting_approval_ = true; } - void clearWaitApproval() { is_waiting_approval_ = false; } - void clearApproval() - { - last_clear_time_ = clock_.now(); - approval_.data = false; - } - -private: - BoolStamped approval_{false}; - bool is_waiting_approval_{true}; - rclcpp::Time last_clear_time_{0, 0, RCL_ROS_TIME}; - - std::string name_; - mutable rclcpp::Clock clock_; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__APPROVAL_HANDLER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 02d8b5daf043d..34dfe30c08965 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -47,12 +47,29 @@ class AvoidanceModule : public SceneModuleInterface bool isExecutionReady() const override; BT::NodeStatus updateState() override; BehaviorModuleOutput plan() override; - PathWithLaneId planCandidate() const override; + CandidateOutput planCandidate() const override; BehaviorModuleOutput planWaitingApproval() override; void onEntry() override; void onExit() override; void updateData() override; + void publishRTCStatus() override + { + rtc_interface_left_.publishCooperateStatus(clock_->now()); + rtc_interface_right_.publishCooperateStatus(clock_->now()); + } + + bool isActivated() const override + { + if (rtc_interface_left_.isRegistered(uuid_left_)) { + return rtc_interface_left_.isActivated(uuid_left_); + } + if (rtc_interface_right_.isRegistered(uuid_right_)) { + return rtc_interface_right_.isActivated(uuid_right_); + } + return false; + } + void setParameters(const AvoidanceParameters & parameters); private: @@ -62,6 +79,34 @@ class AvoidanceModule : public SceneModuleInterface PathShifter path_shifter_; + RTCInterface rtc_interface_left_; + RTCInterface rtc_interface_right_; + UUID uuid_left_; + UUID uuid_right_; + + void updateRTCStatus(const CandidateOutput & candidate) + { + if (candidate.lateral_shift > 0.0) { + rtc_interface_left_.updateCooperateStatus( + uuid_left_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + return; + } + if (candidate.lateral_shift < 0.0) { + rtc_interface_right_.updateCooperateStatus( + uuid_right_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + return; + } + + RCLCPP_WARN_STREAM( + getLogger(), "Direction is UNKNOWN, distance = " << candidate.distance_to_path_change); + } + + void removeRTCStatus() override + { + rtc_interface_left_.clearCooperateStatus(); + rtc_interface_right_.clearCooperateStatus(); + } + // data used in previous planning ShiftedPath prev_output_; ShiftedPath prev_linear_shift_path_; // used for shift point check diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index ab4aeb18ab829..41cbcbbbc1d61 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -88,10 +88,27 @@ class LaneChangeModule : public SceneModuleInterface BT::NodeStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - PathWithLaneId planCandidate() const override; + CandidateOutput planCandidate() const override; void onEntry() override; void onExit() override; + void publishRTCStatus() override + { + rtc_interface_left_.publishCooperateStatus(clock_->now()); + rtc_interface_right_.publishCooperateStatus(clock_->now()); + } + + bool isActivated() const override + { + if (rtc_interface_left_.isRegistered(uuid_left_)) { + return rtc_interface_left_.isActivated(uuid_left_); + } + if (rtc_interface_right_.isRegistered(uuid_right_)) { + return rtc_interface_right_.isActivated(uuid_right_); + } + return false; + } + void setParameters(const LaneChangeParameters & parameters); private: @@ -102,6 +119,48 @@ class LaneChangeModule : public SceneModuleInterface double lane_change_lane_length_{200.0}; double check_distance_{100.0}; + RTCInterface rtc_interface_left_; + RTCInterface rtc_interface_right_; + UUID uuid_left_; + UUID uuid_right_; + + void waitApprovalLeft(const double distance) + { + rtc_interface_left_.updateCooperateStatus( + uuid_left_, isExecutionReady(), distance, clock_->now()); + is_waiting_approval_ = true; + } + + void waitApprovalRight(const double distance) + { + rtc_interface_right_.updateCooperateStatus( + uuid_right_, isExecutionReady(), distance, clock_->now()); + is_waiting_approval_ = true; + } + + void updateRTCStatus(const CandidateOutput & candidate) + { + if (candidate.lateral_shift > 0.0) { + rtc_interface_left_.updateCooperateStatus( + uuid_left_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + return; + } + if (candidate.lateral_shift < 0.0) { + rtc_interface_right_.updateCooperateStatus( + uuid_right_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + return; + } + + RCLCPP_WARN_STREAM( + getLogger(), "Direction is UNKNOWN, distance = " << candidate.distance_to_path_change); + } + + void removeRTCStatus() override + { + rtc_interface_left_.clearCooperateStatus(); + rtc_interface_right_.clearCooperateStatus(); + } + PathWithLaneId getReferencePath() const; lanelet::ConstLanelets getCurrentLanes() const; lanelet::ConstLanelets getLaneChangeLanes( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp index e9f67559df647..503847a2757d6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp @@ -24,6 +24,7 @@ #include #include +#include namespace behavior_path_planner { @@ -47,7 +48,7 @@ class LaneFollowingModule : public SceneModuleInterface bool isExecutionReady() const override; BT::NodeStatus updateState() override; BehaviorModuleOutput plan() override; - PathWithLaneId planCandidate() const override; + CandidateOutput planCandidate() const override; void onEntry() override; void onExit() override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index d23fc3e759e3d..67698d1022255 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -35,6 +35,7 @@ namespace behavior_path_planner { + struct PullOutParameters { double min_stop_distance; @@ -92,7 +93,7 @@ class PullOutModule : public SceneModuleInterface BT::NodeStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - PathWithLaneId planCandidate() const override; + CandidateOutput planCandidate() const override; void onEntry() override; void onExit() override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index d55ab0e695e5e..0121a4286dcbc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -37,6 +37,7 @@ namespace behavior_path_planner { using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + struct PullOverParameters { double min_stop_distance; @@ -91,7 +92,7 @@ class PullOverModule : public SceneModuleInterface BT::NodeStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - PathWithLaneId planCandidate() const override; + CandidateOutput planCandidate() const override; void onEntry() override; void onExit() override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp index 3b78faa8fe1a0..e4a150cc234cb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp @@ -28,7 +28,6 @@ struct SceneModuleStatus { explicit SceneModuleStatus(const std::string & n) : module_name(n) {} std::string module_name; // TODO(Horibe) should be const - bool is_ready{false}; bool is_requested{false}; bool is_waiting_approval{true}; BT::NodeStatus status{BT::NodeStatus::IDLE}; @@ -58,10 +57,6 @@ BT::NodeStatus isExecutionRequested( const std::shared_ptr p, const std::shared_ptr & status); -BT::NodeStatus isExecutionReady( - const std::shared_ptr p, - const std::shared_ptr & status); - } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_BT_NODE_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 82e8e8da1f3f0..4ec7448733652 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -16,11 +16,11 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/scene_module/approval_handler.hpp" #include "behavior_path_planner/utilities.hpp" #include #include +#include #include #include @@ -28,6 +28,7 @@ #include #include #include +#include #include @@ -35,6 +36,7 @@ #include #include +#include #include #include @@ -46,7 +48,9 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using route_handler::LaneChangeDirection; using route_handler::PullOutDirection; using route_handler::PullOverDirection; +using rtc_interface::RTCInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; using PlanResult = PathWithLaneId::SharedPtr; @@ -80,6 +84,15 @@ struct BehaviorModuleOutput TurnSignalInfo turn_signal_info{}; }; +struct CandidateOutput +{ + CandidateOutput() {} + explicit CandidateOutput(const PathWithLaneId & path) : path_candidate{path} {} + PathWithLaneId path_candidate{}; + double lateral_shift{0.0}; + double distance_to_path_change{std::numeric_limits::lowest()}; +}; + class SceneModuleInterface { public: @@ -87,7 +100,8 @@ class SceneModuleInterface : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, - approval_handler_(node), + uuid_(generateUUID()), + is_waiting_approval_{false}, current_state_{BT::NodeStatus::IDLE} { } @@ -124,14 +138,15 @@ class SceneModuleInterface { BehaviorModuleOutput out; out.path = util::generateCenterLinePath(planner_data_); - out.path_candidate = std::make_shared(planCandidate()); + const auto candidate = planCandidate(); + out.path_candidate = std::make_shared(candidate.path_candidate); return out; } /** * @brief Get candidate path. This information is used for external judgement. */ - virtual PathWithLaneId planCandidate() const = 0; + virtual CandidateOutput planCandidate() const = 0; /** * @brief update data for planning. Note that the call of this function does not mean @@ -150,15 +165,13 @@ class SceneModuleInterface updateData(); - if (!approval_handler_.isWaitingApproval()) { + if (!isWaitingApproval()) { return plan(); } // module is waiting approval. Check it. - approval_handler_.setCurrentApproval(planner_data_->approval.is_approved); - if (approval_handler_.isApproved()) { + if (isActivated()) { RCLCPP_DEBUG(logger_, "Was waiting approval, and now approved. Do plan()."); - approval_handler_.clearWaitApproval(); return plan(); } else { RCLCPP_DEBUG(logger_, "keep waiting approval... Do planCandidate()."); @@ -177,15 +190,35 @@ class SceneModuleInterface virtual void onExit() = 0; /** - * @brief set planner data + * @brief Publish status if the module is requested to run */ - void setData(const std::shared_ptr & data) { planner_data_ = data; } + virtual void publishRTCStatus() + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->publishCooperateStatus(clock_->now()); + } - void updateApproval() + /** + * @brief Return true if the activation command is received + */ + virtual bool isActivated() const { - approval_handler_.setCurrentApproval(planner_data_->approval.is_approved); + if (!rtc_interface_ptr_) { + return true; + } + if (rtc_interface_ptr_->isRegistered(uuid_)) { + return rtc_interface_ptr_->isActivated(uuid_); + } + return false; } + /** + * @brief set planner data + */ + void setData(const std::shared_ptr & data) { planner_data_ = data; } + std::string name() const { return name_; } rclcpp::Logger getLogger() const { return logger_; } @@ -201,6 +234,7 @@ class SceneModuleInterface } return debug_avoidance_msg_array_ptr_; } + bool isWaitingApproval() const { return is_waiting_approval_; } private: std::string name_; @@ -211,8 +245,43 @@ class SceneModuleInterface rclcpp::Clock::SharedPtr clock_; mutable AvoidanceDebugMsgArray::SharedPtr debug_avoidance_msg_array_ptr_{}; + std::shared_ptr rtc_interface_ptr_; + UUID uuid_; + bool is_waiting_approval_; + + void updateRTCStatus(const double distance) + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->updateCooperateStatus(uuid_, isExecutionReady(), distance, clock_->now()); + waitApproval(); + } + + virtual void removeRTCStatus() + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->clearCooperateStatus(); + } + + void waitApproval() { is_waiting_approval_ = true; } + + void clearWaitingApproval() { is_waiting_approval_ = false; } + + UUID generateUUID() + { + // Generate random number + UUID uuid; + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid.uuid.begin(), uuid.uuid.end(), bit_eng); + + return uuid; + } + public: - ApprovalHandler approval_handler_; BT::NodeStatus current_state_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index c51f2661c8121..6202f680ce8f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -27,6 +27,7 @@ #include #include #include +#include namespace behavior_path_planner { @@ -65,7 +66,7 @@ class SideShiftModule : public SceneModuleInterface void updateData() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - PathWithLaneId planCandidate() const override; + CandidateOutput planCandidate() const override; void onEntry() override; void onExit() override; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 1b495c6cf636e..078cf718237cb 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -27,6 +27,7 @@ rclcpp rclcpp_components route_handler + rtc_interface sensor_msgs tf2 tf2_geometry_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 8b42353200792..7486b59b15191 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -140,12 +140,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod std::make_shared("LaneChange", *this, lane_change_param); bt_manager_->registerSceneModule(lane_change_module); - auto force_lane_change_module = - std::make_shared("ForceLaneChange", *this, lane_change_param); - bt_manager_->registerSceneModule(force_lane_change_module); - - bt_manager_->registerForceApproval("ForceLaneChange"); - auto pull_over_module = std::make_shared("PullOver", *this, getPullOverParam()); bt_manager_->registerSceneModule(pull_over_module); @@ -573,10 +567,8 @@ void BehaviorPathPlannerNode::run() hazard_signal_publisher_->publish(hazard_signal); } - // for remote operation - publishModuleStatus(bt_manager_->getModulesStatus(), planner_data); + // for debug debug_avoidance_msg_array_publisher_->publish(bt_manager_->getAvoidanceDebugMsgArray()); - publishDebugMarker(bt_manager_->getDebugMarkers()); if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { @@ -615,81 +607,6 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( return path_candidate; } -void BehaviorPathPlannerNode::publishModuleStatus( - const std::vector> & statuses, - const std::shared_ptr planner_data) -{ - auto getModuleType = [](std::string name) { - if (name == "LaneChange") { - return PathChangeModuleId::LANE_CHANGE; - } else if (name == "Avoidance") { - return PathChangeModuleId::AVOIDANCE; - } else if (name == "ForceLaneChange") { - return PathChangeModuleId::FORCE_LANE_CHANGE; - } else if (name == "PullOver") { - return PathChangeModuleId::PULL_OVER; - } else if (name == "PullOut") { - return PathChangeModuleId::PULL_OUT; - } else { - return PathChangeModuleId::NONE; - } - }; - - const auto now = this->now(); - - PathChangeModule ready_module{}; - PathChangeModuleArray running_modules{}; - PathChangeModuleArray force_available{}; - - bool is_ready{false}; - for (auto & status : statuses) { - if (status->status == BT::NodeStatus::RUNNING) { - PathChangeModuleId module{}; - module.type = getModuleType(status->module_name); - running_modules.modules.push_back(module); - } - if (status->module_name == "LaneChange") { - const auto force_approval = planner_data->approval.is_force_approved; - if ( - force_approval.module_name == "ForceLaneChange" && - (now - force_approval.stamp).seconds() < 0.5) { - is_ready = true; - ready_module.module.type = getModuleType("ForceLaneChange"); - } - if (status->is_requested && !status->is_ready) { - PathChangeModuleId module; - module.type = getModuleType("ForceLaneChange"); - force_available.modules.push_back(module); - break; - } - } - if (status->is_ready && status->is_waiting_approval) { - if (status->module_name == "LaneFollowing" || status->module_name == "SideShift") { - continue; - } - is_ready = true; - RCLCPP_DEBUG( - get_logger(), "%s is Ready : ready = %s, is_approved = %s", status->module_name.c_str(), - status->is_ready ? "true" : "false", status->is_waiting_approval ? "true" : "false"); - ready_module.module.type = getModuleType(status->module_name); - } - } - - if (!is_ready) { - prev_ready_module_name_ = "NONE"; - ready_module.module.type = PathChangeModuleId::NONE; - } - - ready_module.header.stamp = now; - plan_ready_publisher_->publish(ready_module); - - running_modules.header.stamp = now; - plan_running_publisher_->publish(running_modules); - - force_available.header.stamp = now; - force_available_publisher_->publish(force_available); -} - void BehaviorPathPlannerNode::publishDebugMarker(const std::vector & debug_markers) { MarkerArray msg{}; diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index cb03e4b5176f1..2f63c708b7b48 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -53,12 +53,10 @@ void BehaviorTreeManager::registerSceneModule(const std::shared_ptrname(); const auto status = std::make_shared(name); - // simple condition node for "isRequested" and "isReady" + // simple condition node for "isRequested" bt_factory_.registerSimpleCondition(name + "_Request", [module, status](BT::TreeNode &) { return isExecutionRequested(module, status); }); - bt_factory_.registerSimpleCondition( - name + "_Ready", [module, status](BT::TreeNode &) { return isExecutionReady(module, status); }); // simple action node for "planCandidate" auto bt_node = @@ -79,13 +77,6 @@ void BehaviorTreeManager::registerSceneModule(const std::shared_ptr & data) { current_planner_data_ = data; @@ -106,6 +97,12 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr RCLCPP_DEBUG(logger_, "BehaviorPathPlanner::run end status = %s", BT::toStr(res).c_str()); + std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { + if (!m->isExecutionRequested()) { + m->onExit(); + } + m->publishRTCStatus(); + }); return output; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 232582f6895d2..bffb62c6d8fcc 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -48,11 +48,14 @@ using tier4_autoware_utils::findNearestIndex; AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, const AvoidanceParameters & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters} +: SceneModuleInterface{name, node}, + parameters_{parameters}, + rtc_interface_left_(node, "avoidance_left"), + rtc_interface_right_(node, "avoidance_right"), + uuid_left_{generateUUID()}, + uuid_right_{generateUUID()} { using std::placeholders::_1; - - approval_handler_.waitApproval(); } bool AvoidanceModule::isExecutionRequested() const @@ -2080,8 +2083,8 @@ BehaviorModuleOutput AvoidanceModule::plan() DEBUG_PRINT("new_shift_points size = %lu", new_shift_points->size()); printShiftPoints(*new_shift_points, "new_shift_points"); addShiftPointIfApproved(*new_shift_points); - } else if (approval_handler_.isWaitingApproval()) { - approval_handler_.clearWaitApproval(); + } else if (isWaitingApproval()) { + clearWaitingApproval(); } // generate path with shift points that have been inserted. @@ -2122,9 +2125,10 @@ BehaviorModuleOutput AvoidanceModule::plan() return output; } -PathWithLaneId AvoidanceModule::planCandidate() const +CandidateOutput AvoidanceModule::planCandidate() const { DEBUG_PRINT("AVOIDANCE planCandidate start"); + CandidateOutput output; auto path_shifter = path_shifter_; auto debug_data = debug_data_; @@ -2140,24 +2144,30 @@ PathWithLaneId AvoidanceModule::planCandidate() const if (new_shift_points) { // clip from shift start index for visualize clipByMinStartIdx(*new_shift_points, shifted_path.path); + output.lateral_shift = new_shift_points->back().getRelativeLength(); + output.distance_to_path_change = new_shift_points->front().start_longitudinal; } clipPathLength(shifted_path.path); - return shifted_path.path; + output.path_candidate = shifted_path.path; + + return output; } BehaviorModuleOutput AvoidanceModule::planWaitingApproval() { // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); - out.path_candidate = std::make_shared(planCandidate()); + const auto candidate = planCandidate(); + out.path_candidate = std::make_shared(candidate.path_candidate); + updateRTCStatus(candidate); return out; } void AvoidanceModule::addShiftPointIfApproved(const AvoidPointArray & shift_points) { - if (approval_handler_.isApproved()) { + if (isActivated()) { DEBUG_PRINT("We want to add this shift point, and approved. ADD SHIFT POINT!"); const size_t prev_size = path_shifter_.getShiftPointsSize(); addNewShiftPoints(path_shifter_, shift_points); @@ -2165,13 +2175,21 @@ void AvoidanceModule::addShiftPointIfApproved(const AvoidPointArray & shift_poin // register original points for consistency registerRawShiftPoints(shift_points); - DEBUG_PRINT("shift_point size: %lu -> %lu", prev_size, path_shifter_.getShiftPointsSize()); + TurnSignalInfo turn_signal_info; + turn_signal_info.signal_distance = std::numeric_limits::lowest(); + if (shift_points.back().getRelativeLength() > 0.0) { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else if (shift_points.back().getRelativeLength() < 0.0) { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + + uuid_left_ = generateUUID(); + uuid_right_ = generateUUID(); - // use this approval. - approval_handler_.clearApproval(); // TODO(Horibe) will be fixed with service-call? + DEBUG_PRINT("shift_point size: %lu -> %lu", prev_size, path_shifter_.getShiftPointsSize()); } else { DEBUG_PRINT("We want to add this shift point, but NOT approved. waiting..."); - approval_handler_.waitApproval(); + waitApproval(); } } @@ -2497,7 +2515,6 @@ void AvoidanceModule::onEntry() DEBUG_PRINT("AVOIDANCE onEntry. wait approval!"); initVariables(); current_state_ = BT::NodeStatus::SUCCESS; - approval_handler_.waitApproval(); } void AvoidanceModule::onExit() @@ -2505,7 +2522,8 @@ void AvoidanceModule::onExit() DEBUG_PRINT("AVOIDANCE onExit"); initVariables(); current_state_ = BT::NodeStatus::IDLE; - approval_handler_.clearWaitApproval(); + clearWaitingApproval(); + removeRTCStatus(); } void AvoidanceModule::setParameters(const AvoidanceParameters & parameters) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 1ac1c7f39b417..442437db94f48 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -37,17 +37,27 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; LaneChangeModule::LaneChangeModule( const std::string & name, rclcpp::Node & node, const LaneChangeParameters & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters} +: SceneModuleInterface{name, node}, + parameters_{parameters}, + rtc_interface_left_(node, "lane_change_left"), + rtc_interface_right_(node, "lane_change_right"), + uuid_left_{generateUUID()}, + uuid_right_{generateUUID()} { - approval_handler_.waitApproval(); } BehaviorModuleOutput LaneChangeModule::run() { RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - approval_handler_.clearWaitApproval(); current_state_ = BT::NodeStatus::RUNNING; - return plan(); + const auto output = plan(); + const auto turn_signal_info = output.turn_signal_info; + if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + waitApprovalLeft(turn_signal_info.signal_distance); + } else if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + waitApprovalRight(turn_signal_info.signal_distance); + } + return output; } void LaneChangeModule::onEntry() @@ -60,12 +70,12 @@ void LaneChangeModule::onEntry() const auto arclength_start = lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); status_.start_distance = arclength_start.length; - approval_handler_.waitApproval(); } void LaneChangeModule::onExit() { - approval_handler_.clearWaitApproval(); + clearWaitingApproval(); + removeRTCStatus(); current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } @@ -162,8 +172,10 @@ BehaviorModuleOutput LaneChangeModule::plan() return output; } -PathWithLaneId LaneChangeModule::planCandidate() const +CandidateOutput LaneChangeModule::planCandidate() const { + CandidateOutput output; + // Get lane change lanes const auto current_lanes = getCurrentLanes(); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); @@ -175,14 +187,26 @@ PathWithLaneId LaneChangeModule::planCandidate() const getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - return selected_path.path; + const auto start_idx = selected_path.shift_point.start_idx; + const auto end_idx = selected_path.shift_point.end_idx; + + output.path_candidate = selected_path.path; + output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - + selected_path.shifted_path.shift_length.at(start_idx); + output.distance_to_path_change = tier4_autoware_utils::calcSignedArcLength( + selected_path.path.points, planner_data_->self_pose->pose.position, + selected_path.shift_point.start.position); + + return output; } BehaviorModuleOutput LaneChangeModule::planWaitingApproval() { BehaviorModuleOutput out; out.path = std::make_shared(getReferencePath()); - out.path_candidate = std::make_shared(planCandidate()); + const auto candidate = planCandidate(); + out.path_candidate = std::make_shared(candidate.path_candidate); + updateRTCStatus(candidate); return out; } @@ -450,6 +474,10 @@ bool LaneChangeModule::isAbortConditionSatisfied() const return false; } + if (!isActivated()) { + return false; + } + // find closest lanelet in original lane lanelet::ConstLanelet closest_lanelet{}; auto clock{rclcpp::Clock{RCL_ROS_TIME}}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index d8a482ef47e1c..8702257e1d774 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -30,7 +30,7 @@ LaneFollowingModule::LaneFollowingModule( void LaneFollowingModule::initParam() { - approval_handler_.clearWaitApproval(); // no need approval + clearWaitingApproval(); // no need approval } bool LaneFollowingModule::isExecutionRequested() const { return true; } @@ -49,7 +49,10 @@ BehaviorModuleOutput LaneFollowingModule::plan() output.path = std::make_shared(getReferencePath()); return output; } -PathWithLaneId LaneFollowingModule::planCandidate() const { return getReferencePath(); } +CandidateOutput LaneFollowingModule::planCandidate() const +{ + return CandidateOutput(getReferencePath()); +} void LaneFollowingModule::onEntry() { initParam(); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 1a8701ff1672d..1ea93635f28da 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -40,12 +40,12 @@ PullOutModule::PullOutModule( const std::string & name, rclcpp::Node & node, const PullOutParameters & parameters) : SceneModuleInterface{name, node}, parameters_{parameters} { - approval_handler_.waitApproval(); + rtc_interface_ptr_ = std::make_shared(node, "pull_out"); } BehaviorModuleOutput PullOutModule::run() { - approval_handler_.clearWaitApproval(); + clearWaitingApproval(); current_state_ = BT::NodeStatus::RUNNING; return plan(); } @@ -62,12 +62,12 @@ void PullOutModule::onEntry() lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); status_.back_finished = false; status_.start_distance = arclength_start.length; - approval_handler_.waitApproval(); } void PullOutModule::onExit() { - approval_handler_.clearWaitApproval(); + clearWaitingApproval(); + removeRTCStatus(); current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "PULL_OUT onExit"); } @@ -173,12 +173,14 @@ BehaviorModuleOutput PullOutModule::plan() return output; } -PathWithLaneId PullOutModule::planCandidate() const +CandidateOutput PullOutModule::planCandidate() const { // Get lane change lanes const auto current_lanes = getCurrentLanes(); const auto shoulder_lanes = getPullOutLanes(current_lanes); + const auto current_pose = planner_data_->self_pose->pose; + // Find pull out path bool found_valid_path, found_safe_path; PullOutPath selected_path; @@ -196,14 +198,21 @@ PathWithLaneId PullOutModule::planCandidate() const if (found_safe_retreat_path == true) { selected_retreat_path.pull_out_path.path.header = planner_data_->route_handler->getRouteHeader(); - return selected_retreat_path.pull_out_path.path; + CandidateOutput output_retreat(selected_retreat_path.pull_out_path.path); + output_retreat.distance_to_path_change = tier4_autoware_utils::calcSignedArcLength( + selected_retreat_path.pull_out_path.path.points, current_pose.position, + selected_retreat_path.backed_pose.position); + return output_retreat; } } } // ROS_ERROR("found safe path in plan candidate :%d", found_safe_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - return selected_path.path; + CandidateOutput output(selected_path.path); + output.distance_to_path_change = tier4_autoware_utils::calcSignedArcLength( + selected_path.path.points, current_pose.position, selected_path.shift_point.start.position); + return output; } BehaviorModuleOutput PullOutModule::planWaitingApproval() @@ -213,23 +222,26 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() const auto current_lanes = getCurrentLanes(); const auto shoulder_lanes = getPullOutLanes(current_lanes); - PathWithLaneId candidatePath; + PathWithLaneId candidate_path; // Generate drivable area { - candidatePath = planCandidate(); + const auto candidate = planCandidate(); + candidate_path = candidate.path_candidate; lanelet::ConstLanelets lanes; lanes.insert(lanes.end(), current_lanes.begin(), current_lanes.end()); lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); const double resolution = common_parameters.drivable_area_resolution; - candidatePath.drivable_area = util::generateDrivableArea( + candidate_path.drivable_area = util::generateDrivableArea( lanes, resolution, common_parameters.vehicle_length, planner_data_); + + updateRTCStatus(candidate.distance_to_path_change); } - for (size_t i = 1; i < candidatePath.points.size(); i++) { - candidatePath.points.at(i).point.longitudinal_velocity_mps = 0.0; + for (size_t i = 1; i < candidate_path.points.size(); i++) { + candidate_path.points.at(i).point.longitudinal_velocity_mps = 0.0; } - out.path = std::make_shared(candidatePath); + out.path = std::make_shared(candidate_path); - out.path_candidate = std::make_shared(planCandidate()); + out.path_candidate = std::make_shared(candidate_path); return out; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 06242c9e92cdf..816750fc73eef 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -39,12 +39,12 @@ PullOverModule::PullOverModule( const std::string & name, rclcpp::Node & node, const PullOverParameters & parameters) : SceneModuleInterface{name, node}, parameters_{parameters} { - approval_handler_.waitApproval(); + rtc_interface_ptr_ = std::make_shared(node, "pull_over"); } BehaviorModuleOutput PullOverModule::run() { - approval_handler_.clearWaitApproval(); + clearWaitingApproval(); current_state_ = BT::NodeStatus::RUNNING; return plan(); } @@ -59,12 +59,12 @@ void PullOverModule::onEntry() const auto arclength_start = lanelet::utils::getArcCoordinates(status_.pull_over_lanes, current_pose); status_.start_distance = arclength_start.length; - approval_handler_.waitApproval(); } void PullOverModule::onExit() { - approval_handler_.clearWaitApproval(); + clearWaitingApproval(); + removeRTCStatus(); current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "PULL_OVER onExit"); } @@ -157,7 +157,7 @@ BehaviorModuleOutput PullOverModule::plan() return output; } -PathWithLaneId PullOverModule::planCandidate() const +CandidateOutput PullOverModule::planCandidate() const { // Get lane change lanes const auto current_lanes = getCurrentLanes(); @@ -170,14 +170,41 @@ PathWithLaneId PullOverModule::planCandidate() const getSafePath(pull_over_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - return selected_path.path; + CandidateOutput output(selected_path.path); + output.distance_to_path_change = tier4_autoware_utils::calcSignedArcLength( + selected_path.path.points, planner_data_->self_pose->pose.position, + selected_path.shift_point.start.position); + + const auto hazard_info = getHazard( + status_.pull_over_lanes, planner_data_->self_pose->pose, + planner_data_->route_handler->getGoalPose(), planner_data_->self_odometry->twist.twist.linear.x, + parameters_.hazard_on_threshold_dis, parameters_.hazard_on_threshold_vel, + planner_data_->parameters.base_link2front); + + const auto turn_info = util::getPathTurnSignal( + status_.current_lanes, status_.pull_over_path.shifted_path, status_.pull_over_path.shift_point, + planner_data_->self_pose->pose, planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->parameters, parameters_.pull_over_search_distance); + + TurnSignalInfo turn_signal_info; + if (hazard_info.first.command == HazardLightsCommand::ENABLE) { + turn_signal_info.hazard_signal.command = hazard_info.first.command; + turn_signal_info.signal_distance = hazard_info.second; + } else { + turn_signal_info.turn_signal.command = turn_info.first.command; + turn_signal_info.signal_distance = turn_info.second; + } + + return output; } BehaviorModuleOutput PullOverModule::planWaitingApproval() { BehaviorModuleOutput out; out.path = std::make_shared(getReferencePath()); - out.path_candidate = std::make_shared(planCandidate()); + const auto candidate = planCandidate(); + out.path_candidate = std::make_shared(candidate.path_candidate); + updateRTCStatus(candidate.distance_to_path_change); return out; } diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp index 7a3118e9ca1f8..ea1411024e60d 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp @@ -29,16 +29,6 @@ BT::NodeStatus isExecutionRequested( return ret ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } -BT::NodeStatus isExecutionReady( - const std::shared_ptr p, - const std::shared_ptr & status) -{ - const auto ret = p->isExecutionReady(); - status->is_ready = ret; - RCLCPP_DEBUG_STREAM(p->getLogger(), "name = " << p->name() << ", result = " << ret); - return ret ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; -} - SceneModuleBTNodeInterface::SceneModuleBTNodeInterface( const std::string & name, const BT::NodeConfiguration & config, const std::shared_ptr & scene_module, @@ -54,11 +44,11 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() auto current_status = BT::NodeStatus::RUNNING; scene_module_->onEntry(); - scene_module_->updateApproval(); - const bool is_waiting_approval = scene_module_->approval_handler_.isWaitingApproval() && - !scene_module_->approval_handler_.isApproved(); - if (is_waiting_approval) { + const bool is_lane_following = scene_module_->name() == "LaneFollowing"; + + const bool is_waiting_approval = !scene_module_->isActivated(); + if (is_waiting_approval && !is_lane_following) { try { // NOTE: Since BehaviorTreeCpp has an issue to shadow the exception reason thrown // in the TreeNode, catch and display it here until the issue is fixed. @@ -67,7 +57,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() if (!res) { RCLCPP_ERROR_STREAM(scene_module_->getLogger(), "setOutput() failed : " << res.error()); } - module_status_->is_waiting_approval = scene_module_->approval_handler_.isWaitingApproval(); + module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( scene_module_->getLogger(), "behavior module has failed with exception: " << e.what()); @@ -89,7 +79,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() // for data output module_status_->status = current_status; - module_status_->is_waiting_approval = scene_module_->approval_handler_.isWaitingApproval(); + module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( scene_module_->getLogger(), "behavior module has failed with exception: " << e.what()); diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index ca01e33ca874f..cbbc7c5604efc 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -64,7 +64,7 @@ SideShiftModule::SideShiftModule( "~/input/lateral_offset", 1, std::bind(&SideShiftModule::onLateralOffset, this, _1)); // If lateral offset is subscribed, it approves side shift module automatically - approval_handler_.clearWaitApproval(); + clearWaitingApproval(); } void SideShiftModule::initVariables() @@ -273,7 +273,7 @@ BehaviorModuleOutput SideShiftModule::plan() return output; } -PathWithLaneId SideShiftModule::planCandidate() const +CandidateOutput SideShiftModule::planCandidate() const { auto path_shifter_local = path_shifter_; @@ -286,7 +286,7 @@ PathWithLaneId SideShiftModule::planCandidate() const // Reset orientation setOrientation(&shifted_path.path); - return shifted_path.path; + return CandidateOutput(shifted_path.path); } BehaviorModuleOutput SideShiftModule::planWaitingApproval() @@ -302,7 +302,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() BehaviorModuleOutput output; output.path = std::make_shared(shifted_path.path); - output.path_candidate = std::make_shared(planCandidate()); + output.path_candidate = std::make_shared(planCandidate().path_candidate); prev_output_ = shifted_path;