diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 4463620dcc4a9..bf135003fd353 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,21 +7,26 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -set(COMPILE_WITH_OLD_ARCHITECTURE FALSE) - -set(common_src +ament_auto_add_library(behavior_path_planner_node SHARED + src/planner_manager.cpp src/steering_factor_interface.cpp src/behavior_path_planner_node.cpp src/scene_module/scene_module_visitor.cpp src/scene_module/avoidance/avoidance_module.cpp + src/scene_module/avoidance/manager.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp + src/scene_module/dynamic_avoidance/manager.cpp src/scene_module/start_planner/start_planner_module.cpp + src/scene_module/start_planner/manager.cpp src/scene_module/goal_planner/goal_planner_module.cpp + src/scene_module/goal_planner/manager.cpp src/scene_module/side_shift/side_shift_module.cpp + src/scene_module/side_shift/manager.cpp src/scene_module/lane_change/interface.cpp src/scene_module/lane_change/normal.cpp src/scene_module/lane_change/external_request.cpp src/scene_module/lane_change/avoidance_by_lane_change.cpp + src/scene_module/lane_change/manager.cpp src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp @@ -50,33 +55,6 @@ set(common_src src/marker_util/lane_change/debug.cpp ) -if(COMPILE_WITH_OLD_ARCHITECTURE) - ament_auto_add_library(behavior_path_planner_node SHARED - src/behavior_tree_manager.cpp - src/scene_module/scene_module_bt_node_interface.cpp - src/scene_module/lane_following/lane_following_module.cpp - ${common_src} - ) - - target_compile_definitions(behavior_path_planner_node PRIVATE USE_OLD_ARCHITECTURE) - - message(WARNING "Build behavior_path_planner with OLD framework...") - -else() - ament_auto_add_library(behavior_path_planner_node SHARED - src/planner_manager.cpp - src/scene_module/avoidance/manager.cpp - src/scene_module/dynamic_avoidance/manager.cpp - src/scene_module/start_planner/manager.cpp - src/scene_module/goal_planner/manager.cpp - src/scene_module/side_shift/manager.cpp - src/scene_module/lane_change/manager.cpp - ${common_src} - ) - - message(WARNING "Build behavior_path_planner with NEW framework...") -endif() - target_include_directories(behavior_path_planner_node SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) 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 d06a3aae68e4f..454106e4e6811 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 @@ -16,26 +16,14 @@ #define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" - -#ifdef USE_OLD_ARCHITECTURE -#include "behavior_path_planner/behavior_tree_manager.hpp" -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" -#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" -#include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" -#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" -#include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" -#else #include "behavior_path_planner/planner_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/manager.hpp" #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/side_shift/manager.hpp" #include "behavior_path_planner/scene_module/start_planner/manager.hpp" -#endif - #include "behavior_path_planner/steering_factor_interface.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" @@ -127,11 +115,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_data_; -#ifdef USE_OLD_ARCHITECTURE - std::shared_ptr bt_manager_; -#else std::shared_ptr planner_manager_; -#endif std::unique_ptr steering_factor_interface_ptr_; Scenario::SharedPtr current_scenario_{nullptr}; @@ -160,10 +144,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node BehaviorPathPlannerParameters getCommonParam(); -#ifdef USE_OLD_ARCHITECTURE - BehaviorTreeManagerParam getBehaviorTreeManagerParam(); -#endif - AvoidanceParameters getAvoidanceParam(); DynamicAvoidanceParameters getDynamicAvoidanceParam(); LaneChangeParameters getLaneChangeParam(); @@ -196,15 +176,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ -#ifdef USE_OLD_ARCHITECTURE - PathWithLaneId::SharedPtr getPath( - const BehaviorModuleOutput & bt_out, const std::shared_ptr & planner_data, - const std::shared_ptr & bt_manager); -#else PathWithLaneId::SharedPtr getPath( const BehaviorModuleOutput & bt_out, const std::shared_ptr & planner_data, const std::shared_ptr & planner_manager); -#endif bool keepInputPoints(const std::vector> & statuses) const; @@ -246,11 +220,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish path candidate */ -#ifdef USE_OLD_ARCHITECTURE - void publishPathCandidate( - const std::vector> & scene_modules, - const std::shared_ptr & planner_data); -#else void publishPathCandidate( const std::vector> & managers, const std::shared_ptr & planner_data); @@ -258,7 +227,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node void publishPathReference( const std::vector> & managers, const std::shared_ptr & planner_data); -#endif /** * @brief convert path with lane id to path for publish path candidate 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 deleted file mode 100644 index f179b90139705..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp +++ /dev/null @@ -1,90 +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__BEHAVIOR_TREE_MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__BEHAVIOR_TREE_MANAGER_HPP_ - -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace behavior_path_planner -{ -struct BehaviorTreeManagerParam -{ - std::string bt_tree_config_path; - int groot_zmq_publisher_port; - int groot_zmq_server_port; -}; - -class BehaviorTreeManager -{ -public: - BehaviorTreeManager(rclcpp::Node & node, const BehaviorTreeManagerParam & param); - void createBehaviorTree(); - void registerSceneModule(const std::shared_ptr & p); - - void resetBehaviorTree(); - - BehaviorModuleOutput run(const std::shared_ptr & data); - std::vector> getModulesStatus(); - std::shared_ptr getAllSceneModuleDebugMsgData(); - std::vector> getSceneModules() const - { - return scene_modules_; - } - - AvoidanceDebugMsgArray getAvoidanceDebugMsgArray(); - -private: - BehaviorTreeManagerParam bt_manager_param_; - std::shared_ptr current_planner_data_; - std::vector> scene_modules_; - std::vector> modules_status_; - rclcpp::Logger logger_; - rclcpp::Clock clock_; - std::shared_ptr scene_module_visitor_ptr_; - - BT::BehaviorTreeFactory bt_factory_; - BT::Tree bt_tree_; - BT::Blackboard::Ptr blackboard_; - - void resetNotRunningModulePathCandidate(); - - // For Groot monitoring - std::unique_ptr groot_monitor_; - - void addGrootMonitoring( - BT::Tree * tree, uint16_t publisher_port, uint16_t server_port, - uint16_t max_msg_per_second = 25); - - void resetGrootMonitor(); -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__BEHAVIOR_TREE_MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp b/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp index cfb5209ff795c..f6a7a10b21db1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp @@ -15,16 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ #define BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ -#ifdef USE_OLD_ARCHITECTURE -#include -#endif - namespace behavior_path_planner { - -#ifdef USE_OLD_ARCHITECTURE -using ModuleStatus = BT::NodeStatus; -#else enum class ModuleStatus { IDLE = 0, RUNNING = 1, @@ -32,8 +24,6 @@ enum class ModuleStatus { FAILURE = 3, // SKIPPED = 4, }; -#endif - } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__MODULE_STATUS_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 28357af3cda84..5751e1873a579 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 @@ -46,14 +46,9 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsg; class AvoidanceModule : public SceneModuleInterface { public: -#ifdef USE_OLD_ARCHITECTURE - AvoidanceModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); -#else AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map > & rtc_interface_ptr_map); -#endif ModuleStatus updateState() override; ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } @@ -67,12 +62,10 @@ class AvoidanceModule : public SceneModuleInterface void updateData() override; void acceptVisitor(const std::shared_ptr & visitor) const override; -#ifndef USE_OLD_ARCHITECTURE void updateModuleParams(const std::shared_ptr & parameters) { parameters_ = parameters; } -#endif std::shared_ptr get_debug_msg_array() const; private: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 4898188c71319..32735b72913fa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -109,11 +109,6 @@ class DynamicAvoidanceModule : public SceneModuleInterface } }; -#ifdef USE_OLD_ARCHITECTURE - DynamicAvoidanceModule( - const std::string & name, rclcpp::Node & node, - std::shared_ptr parameters); -#else DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, @@ -123,7 +118,6 @@ class DynamicAvoidanceModule : public SceneModuleInterface { parameters_ = parameters; } -#endif bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index da2665723ae90..1e4a1370ac867 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -87,11 +87,6 @@ struct PUllOverStatus class GoalPlannerModule : public SceneModuleInterface { public: -#ifdef USE_OLD_ARCHITECTURE - GoalPlannerModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters); -#else GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -101,7 +96,6 @@ class GoalPlannerModule : public SceneModuleInterface { parameters_ = parameters; } -#endif BehaviorModuleOutput run() override; bool isExecutionRequested() const override; @@ -152,10 +146,7 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; -// save last time and pose -#ifdef USE_OLD_ARCHITECTURE - std::unique_ptr last_received_time_; -#endif + // save last time and pose std::unique_ptr last_approved_time_; std::unique_ptr last_increment_time_; std::unique_ptr last_path_update_time_; 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 deleted file mode 100644 index d908dea155180..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp +++ /dev/null @@ -1,58 +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__LANE_FOLLOWING__LANE_FOLLOWING_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_FOLLOWING__LANE_FOLLOWING_MODULE_HPP_ - -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" - -#include - -#include - -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; - -class LaneFollowingModule : public SceneModuleInterface -{ -public: - LaneFollowingModule(const std::string & name, rclcpp::Node & node); - - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - BT::NodeStatus updateState() override; - BehaviorModuleOutput plan() override; - CandidateOutput planCandidate() const override; - void processOnEntry() override; - void processOnExit() override; - - void setParameters(const std::shared_ptr & parameters); - void acceptVisitor( - [[maybe_unused]] const std::shared_ptr & visitor) const override - { - } - -private: - BehaviorModuleOutput getReferencePath() const; - void initParam(); -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_FOLLOWING__LANE_FOLLOWING_MODULE_HPP_ 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 deleted file mode 100644 index e3578928782f3..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp +++ /dev/null @@ -1,63 +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__SCENE_MODULE_BT_NODE_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_BT_NODE_INTERFACE_HPP_ - -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" - -#include - -#include -#include - -namespace behavior_path_planner -{ -struct SceneModuleStatus -{ - explicit SceneModuleStatus(const std::string & n) : module_name(n) {} - std::string module_name; // TODO(Horibe) should be const - bool is_requested{false}; - bool is_execution_ready{false}; - bool is_waiting_approval{false}; - BT::NodeStatus status{BT::NodeStatus::SUCCESS}; -}; - -class SceneModuleBTNodeInterface : public BT::CoroActionNode -{ -public: - SceneModuleBTNodeInterface( - const std::string & name, const BT::NodeConfiguration & config, - const std::shared_ptr & scene_module, - const std::shared_ptr & module_status); - -protected: - std::shared_ptr scene_module_; - std::shared_ptr module_status_; - -public: - BT::NodeStatus tick() override; - void halt() override; - static BT::PortsList providedPorts(); - - BT::NodeStatus planCandidate(BT::TreeNode & self); -}; - -BT::NodeStatus isExecutionRequested( - 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 68db46e68b1d3..f068bff5e8caa 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 @@ -76,37 +76,11 @@ class SceneModuleInterface clock_{node.get_clock()}, is_waiting_approval_{false}, is_locked_new_module_launch_{false}, -#ifdef USE_OLD_ARCHITECTURE - current_state_{ModuleStatus::SUCCESS}, -#else current_state_{ModuleStatus::IDLE}, -#endif rtc_interface_ptr_map_(rtc_interface_ptr_map), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) { -#ifdef USE_OLD_ARCHITECTURE - { - const auto ns = std::string("~/debug/") + utils::convertToSnakeCase(name); - pub_debug_marker_ = node.create_publisher(ns, 20); - } - - { - const auto ns = std::string("~/info/") + utils::convertToSnakeCase(name); - pub_info_marker_ = node.create_publisher(ns, 20); - } - - { - const auto ns = std::string("~/virtual_wall/") + utils::convertToSnakeCase(name); - pub_virtual_wall_ = node.create_publisher(ns, 20); - } - - { - const auto ns = std::string("~/output/stop_reasons"); - pub_stop_reasons_ = node.create_publisher(ns, 20); - } -#endif - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { uuid_map_.emplace(itr->first, generateUUID()); } @@ -185,10 +159,6 @@ class SceneModuleInterface */ virtual BehaviorModuleOutput run() { -#ifdef USE_OLD_ARCHITECTURE - current_state_ = ModuleStatus::RUNNING; -#endif - updateData(); if (!isWaitingApproval()) { @@ -212,11 +182,7 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); -#ifdef USE_OLD_ARCHITECTURE - current_state_ = ModuleStatus::SUCCESS; -#else current_state_ = ModuleStatus::IDLE; -#endif stop_reason_ = StopReason(); @@ -314,61 +280,6 @@ class SceneModuleInterface */ virtual void setData(const std::shared_ptr & data) { planner_data_ = data; } -#ifdef USE_OLD_ARCHITECTURE - void publishInfoMarker() { pub_info_marker_->publish(info_marker_); } - - void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } - - void publishStopReasons() - { - StopReasonArray stop_reason_array; - stop_reason_array.header.frame_id = "map"; - stop_reason_array.header.stamp = clock_->now(); - - const auto reason = getStopReason(); - if (reason.reason == "") { - return; - } - - stop_reason_array.stop_reasons.push_back(reason); - - pub_stop_reasons_->publish(stop_reason_array); - } - - void publishVirtualWall() - { - MarkerArray markers{}; - - const auto opt_stop_pose = getStopPose(); - if (!!opt_stop_pose) { - const auto virtual_wall = createStopVirtualWallMarker( - opt_stop_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); - appendMarkerArray(virtual_wall, &markers); - } - - const auto opt_slow_pose = getSlowPose(); - if (!!opt_slow_pose) { - const auto virtual_wall = createSlowDownVirtualWallMarker( - opt_slow_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); - appendMarkerArray(virtual_wall, &markers); - } - - const auto opt_dead_pose = getDeadPose(); - if (!!opt_dead_pose) { - const auto virtual_wall = createDeadLineVirtualWallMarker( - opt_dead_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); - appendMarkerArray(virtual_wall, &markers); - } - - const auto module_specific_wall = getModuleVirtualWall(); - appendMarkerArray(module_specific_wall, &markers); - - pub_virtual_wall_->publish(markers); - - resetWallPoses(); - } -#endif - bool isWaitingApproval() const { return is_waiting_approval_; } bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } @@ -459,13 +370,6 @@ class SceneModuleInterface rclcpp::Logger logger_; -#ifdef USE_OLD_ARCHITECTURE - rclcpp::Publisher::SharedPtr pub_info_marker_; - rclcpp::Publisher::SharedPtr pub_debug_marker_; - rclcpp::Publisher::SharedPtr pub_virtual_wall_; - rclcpp::Publisher::SharedPtr pub_stop_reasons_; -#endif - BehaviorModuleOutput previous_module_output_; protected: @@ -575,11 +479,7 @@ class SceneModuleInterface PlanResult path_candidate_; PlanResult path_reference_; -#ifdef USE_OLD_ARCHITECTURE - ModuleStatus current_state_{ModuleStatus::SUCCESS}; -#else ModuleStatus current_state_{ModuleStatus::IDLE}; -#endif StopReason stop_reason_; 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 250ece8190e37..8118d05ef225d 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 @@ -40,16 +40,10 @@ using tier4_planning_msgs::msg::LateralOffset; class SideShiftModule : public SceneModuleInterface { public: -#ifdef USE_OLD_ARCHITECTURE - SideShiftModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters); -#else SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map); -#endif bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -81,10 +75,6 @@ class SideShiftModule : public SceneModuleInterface void initVariables(); -#ifdef USE_OLD_ARCHITECTURE - void onLateralOffset(const LateralOffset::ConstSharedPtr lateral_offset_msg); -#endif - // non-const methods BehaviorModuleOutput adjustDrivableArea(const ShiftedPath & path) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index a48c24b183111..6aa3ca9ac2239 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -66,11 +66,6 @@ struct PullOutStatus class StartPlannerModule : public SceneModuleInterface { public: -#ifdef USE_OLD_ARCHITECTURE - StartPlannerModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters); -#else StartPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -80,7 +75,6 @@ class StartPlannerModule : public SceneModuleInterface { parameters_ = parameters; } -#endif BehaviorModuleOutput run() override; @@ -156,11 +150,6 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedCurrentPath(); void setDebugData() const; - -// temporary for old architecture -#ifdef USE_OLD_ARCHITECTURE - mutable bool is_executed_{false}; -#endif }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp index 568ecdc98c705..40c339e507683 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -32,10 +32,6 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase BehaviorModuleOutput plan(const std::shared_ptr & planner_data) const override; protected: -#ifdef USE_OLD_ARCHITECTURE - boost::optional getReferencePath( - const std::shared_ptr & planner_data) const; -#endif PathWithLaneId modifyPathForSmoothGoalConnection( const PathWithLaneId & path, const std::shared_ptr & planner_data) const; }; 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 4ed96ebe463ae..410c443b94ed9 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -137,66 +137,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod m_set_param_res = this->add_on_set_parameters_callback( std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); -#ifdef USE_OLD_ARCHITECTURE - // behavior tree manager - { - RCLCPP_INFO(get_logger(), "use behavior tree."); - - const std::string path_candidate_name_space = "/planning/path_candidate/"; - const std::lock_guard lock(mutex_manager_); // for bt_manager_ - - bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); - - auto side_shift_module = - std::make_shared("SideShift", *this, side_shift_param_ptr_); - bt_manager_->registerSceneModule(side_shift_module); - - auto avoidance_module = - std::make_shared("Avoidance", *this, avoidance_param_ptr_); - path_candidate_publishers_.emplace( - "Avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); - bt_manager_->registerSceneModule(avoidance_module); - - auto lane_following_module = std::make_shared("LaneFollowing", *this); - bt_manager_->registerSceneModule(lane_following_module); - - auto ext_request_lane_change_right_module = - std::make_shared( - "ExternalRequestLaneChangeRight", *this, lane_change_param_ptr_); - path_candidate_publishers_.emplace( - "ExternalRequestLaneChangeRight", - create_publisher(path_candidate_name_space + "external_request_lane_change_right", 1)); - bt_manager_->registerSceneModule(ext_request_lane_change_right_module); - - auto ext_request_lane_change_left_module = - std::make_shared( - "ExternalRequestLaneChangeLeft", *this, lane_change_param_ptr_); - path_candidate_publishers_.emplace( - "ExternalRequestLaneChangeLeft", - create_publisher(path_candidate_name_space + "external_request_lane_change_left", 1)); - bt_manager_->registerSceneModule(ext_request_lane_change_left_module); - - auto lane_change_module = - std::make_shared("LaneChange", *this, lane_change_param_ptr_); - path_candidate_publishers_.emplace( - "LaneChange", create_publisher(path_candidate_name_space + "lane_change", 1)); - bt_manager_->registerSceneModule(lane_change_module); - - auto goal_planner = - std::make_shared("GoalPlanner", *this, goal_planner_param_ptr_); - path_candidate_publishers_.emplace( - "GoalPlanner", create_publisher(path_candidate_name_space + "goal_planner", 1)); - bt_manager_->registerSceneModule(goal_planner); - - auto start_planner_module = - std::make_shared("StartPlanner", *this, start_planner_param_ptr_); - path_candidate_publishers_.emplace( - "StartPlanner", create_publisher(path_candidate_name_space + "start_planner", 1)); - bt_manager_->registerSceneModule(start_planner_module); - - bt_manager_->createBehaviorTree(); - } -#else { RCLCPP_INFO(get_logger(), "not use behavior tree."); @@ -308,7 +248,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_->registerSceneModuleManager(manager); } } -#endif // turn signal decider { @@ -335,19 +274,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() { -#ifdef USE_OLD_ARCHITECTURE - auto registered_modules_ptr = bt_manager_->getSceneModules(); - std::vector waiting_approval_modules; - for (const auto & module : registered_modules_ptr) { - if (module->isWaitingApproval() == true) { - waiting_approval_modules.push_back(module->name()); -#else auto all_scene_module_ptr = planner_manager_->getSceneModuleStatus(); std::vector waiting_approval_modules; for (const auto & module : all_scene_module_ptr) { if (module->is_waiting_approval == true) { waiting_approval_modules.push_back(module->module_name); -#endif } } return waiting_approval_modules; @@ -1126,17 +1057,6 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam() return p; } -#ifdef USE_OLD_ARCHITECTURE -BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() -{ - BehaviorTreeManagerParam p{}; - p.bt_tree_config_path = declare_parameter("bt_tree_config_path"); - p.groot_zmq_publisher_port = declare_parameter("groot_zmq_publisher_port"); - p.groot_zmq_server_port = declare_parameter("groot_zmq_server_port"); - return p; -} -#endif - // wait until mandatory data is ready bool BehaviorPathPlannerNode::isDataReady() { @@ -1232,9 +1152,7 @@ void BehaviorPathPlannerNode::run() const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); if (route_ptr) { planner_data_->route_handler->setRoute(*route_ptr); -#ifndef USE_OLD_ARCHITECTURE planner_manager_->resetRootLanelet(planner_data_); -#endif // uuid is not changed when rerouting with modified goal, // in this case do not need to rest modules. @@ -1244,36 +1162,22 @@ void BehaviorPathPlannerNode::run() // so that the each modules do not have to care about the "route jump". if (!is_first_time && !has_same_route_id) { RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); -#ifdef USE_OLD_ARCHITECTURE - bt_manager_->resetBehaviorTree(); -#else planner_manager_->reset(); -#endif } } -#ifndef USE_OLD_ARCHITECTURE const auto controlled_by_autoware_autonomously = planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && planner_data_->operation_mode->is_autoware_control_enabled; if (!controlled_by_autoware_autonomously) { planner_manager_->resetRootLanelet(planner_data_); } -#endif // run behavior planner -#ifdef USE_OLD_ARCHITECTURE - const auto output = bt_manager_->run(planner_data_); -#else const auto output = planner_manager_->run(planner_data_); -#endif // path handling -#ifdef USE_OLD_ARCHITECTURE - const auto path = getPath(output, planner_data_, bt_manager_); -#else const auto path = getPath(output, planner_data_, planner_manager_); -#endif // update planner data planner_data_->prev_output_path = path; @@ -1305,19 +1209,10 @@ void BehaviorPathPlannerNode::run() get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); } -#ifdef USE_OLD_ARCHITECTURE - publishPathCandidate(bt_manager_->getSceneModules(), planner_data_); - publishSceneModuleDebugMsg(bt_manager_->getAllSceneModuleDebugMsgData()); -#else publishSceneModuleDebugMsg(planner_manager_->getDebugMsg()); publishPathCandidate(planner_manager_->getSceneModuleManagers(), planner_data_); publishPathReference(planner_manager_->getSceneModuleManagers(), planner_data_); stop_reason_publisher_->publish(planner_manager_->getStopReasons()); -#endif - -#ifdef USE_OLD_ARCHITECTURE - lk_manager.unlock(); // release bt_manager_ -#endif if (output.modified_goal) { PoseWithUuidStamped modified_goal = *(output.modified_goal); @@ -1336,12 +1231,10 @@ void BehaviorPathPlannerNode::run() lk_pd.unlock(); // release planner_data_ -#ifndef USE_OLD_ARCHITECTURE planner_manager_->print(); planner_manager_->publishMarker(); planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ -#endif RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } @@ -1518,20 +1411,6 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg( } } -#ifdef USE_OLD_ARCHITECTURE -void BehaviorPathPlannerNode::publishPathCandidate( - const std::vector> & scene_modules, - const std::shared_ptr & planner_data) -{ - for (auto & module : scene_modules) { - if (path_candidate_publishers_.count(module->name()) != 0) { - path_candidate_publishers_.at(module->name()) - ->publish( - convertToPath(module->getPathCandidate(), module->isExecutionReady(), planner_data)); - } - } -} -#else void BehaviorPathPlannerNode::publishPathCandidate( const std::vector> & managers, const std::shared_ptr & planner_data) @@ -1583,7 +1462,6 @@ void BehaviorPathPlannerNode::publishPathReference( } } } -#endif Path BehaviorPathPlannerNode::convertToPath( const std::shared_ptr & path_candidate_ptr, const bool is_ready, @@ -1611,15 +1489,9 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } -#ifdef USE_OLD_ARCHITECTURE -PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( - const BehaviorModuleOutput & bt_output, const std::shared_ptr & planner_data, - const std::shared_ptr & bt_manager) -#else PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( const BehaviorModuleOutput & bt_output, const std::shared_ptr & planner_data, const std::shared_ptr & planner_manager) -#endif { // TODO(Horibe) do some error handling when path is not available. @@ -1630,11 +1502,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); PathWithLaneId connected_path; -#ifdef USE_OLD_ARCHITECTURE - const auto module_status_ptr_vec = bt_manager->getModulesStatus(); -#else const auto module_status_ptr_vec = planner_manager->getSceneModuleStatus(); -#endif const auto resampled_path = utils::resamplePathWithSpline( *path, planner_data->parameters.output_path_interval, keepInputPoints(module_status_ptr_vec)); @@ -1645,11 +1513,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( bool BehaviorPathPlannerNode::keepInputPoints( const std::vector> & statuses) const { -#ifdef USE_OLD_ARCHITECTURE - const std::vector target_modules = {"GoalPlanner", "Avoidance"}; -#else const std::vector target_modules = {"goal_planner", "avoidance"}; -#endif const auto target_status = ModuleStatus::RUNNING; @@ -1745,12 +1609,10 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( return result; } -#ifndef USE_OLD_ARCHITECTURE { const std::lock_guard lock(mutex_manager_); // for planner_manager_ planner_manager_->updateModuleParams(parameters); } -#endif result.successful = true; result.reason = "success"; diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp deleted file mode 100644 index 2e8627b745db8..0000000000000 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ /dev/null @@ -1,175 +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. - -#include "behavior_path_planner/behavior_tree_manager.hpp" - -#include "behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/utils.hpp" - -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -BehaviorTreeManager::BehaviorTreeManager( - rclcpp::Node & node, const BehaviorTreeManagerParam & param) -: bt_manager_param_(param), - logger_(node.get_logger().get_child("behavior_tree_manager")), - clock_(*node.get_clock()) -{ -} - -void BehaviorTreeManager::createBehaviorTree() -{ - blackboard_ = BT::Blackboard::create(); - try { - bt_tree_ = bt_factory_.createTreeFromFile(bt_manager_param_.bt_tree_config_path, blackboard_); - } catch (...) { - RCLCPP_ERROR( - logger_, "Failed to create BT from: %s", bt_manager_param_.bt_tree_config_path.c_str()); - exit(EXIT_FAILURE); - } - addGrootMonitoring( - &bt_tree_, bt_manager_param_.groot_zmq_publisher_port, bt_manager_param_.groot_zmq_server_port); -} - -void BehaviorTreeManager::registerSceneModule(const std::shared_ptr & module) -{ - const std::string & name = module->name(); - const auto status = std::make_shared(name); - - // simple condition node for "isRequested" - bt_factory_.registerSimpleCondition(name + "_Request", [module, status](BT::TreeNode &) { - return isExecutionRequested(module, status); - }); - - // simple action node for "planCandidate" - auto bt_node = - std::make_shared("", BT::NodeConfiguration{}, module, status); - bt_factory_.registerSimpleAction( - name + "_PlanCandidate", - [bt_node](BT::TreeNode & tree_node) { return bt_node->planCandidate(tree_node); }, - SceneModuleBTNodeInterface::providedPorts()); - - // register builder with default tick functor for "plan" - auto builder = [module, status]( - const std::string & _name, const BT::NodeConfiguration & _config) { - return std::make_unique(_name, _config, module, status); - }; - bt_factory_.registerBuilder(name + "_Plan", builder); - - scene_modules_.push_back(module); - modules_status_.push_back(status); -} - -BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr & data) -{ - current_planner_data_ = data; - - // set planner_data & reset status - std::for_each( - scene_modules_.begin(), scene_modules_.end(), [&data](const auto & m) { m->setData(data); }); - std::for_each(modules_status_.begin(), modules_status_.end(), [](const auto & s) { - *s = SceneModuleStatus{s->module_name}; - }); - - const bool is_any_module_running = std::any_of( - scene_modules_.begin(), scene_modules_.end(), - [](const auto & module) { return module->getCurrentStatus() == BT::NodeStatus::RUNNING; }); - - if ( - !is_any_module_running && - utils::isEgoOutOfRoute( - data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler)) { - BehaviorModuleOutput output = utils::createGoalAroundPath(data); - return output; - } - - // reset blackboard - blackboard_->set("output", BehaviorModuleOutput{}); - - const auto res = bt_tree_.tickRoot(); - - const auto output = blackboard_->get("output"); - - RCLCPP_DEBUG(logger_, "BehaviorPathPlanner::run end status = %s", BT::toStr(res).c_str()); - - resetNotRunningModulePathCandidate(); - - std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { - m->publishInfoMarker(); - m->publishDebugMarker(); - m->publishStopReasons(); - m->publishVirtualWall(); - if (!m->isExecutionRequested()) { - m->onExit(); - } - m->publishRTCStatus(); - m->publishSteeringFactor(); - }); - return output; -} - -std::vector> -BehaviorTreeManager::getModulesStatus() -{ - return modules_status_; -} - -void BehaviorTreeManager::resetNotRunningModulePathCandidate() -{ - const bool is_any_module_running = std::any_of( - scene_modules_.begin(), scene_modules_.end(), - [](const auto & module) { return module->getCurrentStatus() == BT::NodeStatus::RUNNING; }); - - for (auto & module : scene_modules_) { - if (is_any_module_running && (module->getCurrentStatus() != BT::NodeStatus::RUNNING)) { - module->resetPathCandidate(); - } - } -} - -void BehaviorTreeManager::resetBehaviorTree() -{ - bt_tree_.haltTree(); -} - -void BehaviorTreeManager::addGrootMonitoring( - BT::Tree * tree, uint16_t publisher_port, uint16_t server_port, uint16_t max_msg_per_second) -{ - groot_monitor_ = - std::make_unique(*tree, max_msg_per_second, publisher_port, server_port); -} - -void BehaviorTreeManager::resetGrootMonitor() -{ - if (groot_monitor_) { - groot_monitor_.reset(); - } -} - -std::shared_ptr BehaviorTreeManager::getAllSceneModuleDebugMsgData() -{ - scene_module_visitor_ptr_ = std::make_shared(); - for (const auto & module : scene_modules_) { - module->acceptVisitor(scene_module_visitor_ptr_); - } - return scene_module_visitor_ptr_; -} -} // namespace behavior_path_planner 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 91f1c23c3008d..587afb1fdee57 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 @@ -96,20 +96,12 @@ bool isDrivingSameLane( } } // namespace -#ifdef USE_OLD_ARCHITECTURE -AvoidanceModule::AvoidanceModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {"left", "right"})}, - parameters_{parameters}, - helper_{parameters} -#else AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, helper_{parameters} -#endif { } @@ -122,26 +114,11 @@ bool AvoidanceModule::isExecutionRequested() const } // Check ego is in preferred lane -#ifdef USE_OLD_ARCHITECTURE - const auto avoid_data = calcAvoidancePlanningData(debug_data_); - - const auto current_lanes = utils::getCurrentLanes(planner_data_); - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet( - current_lanes, planner_data_->self_odometry->pose.pose, ¤t_lane); - const auto num = planner_data_->route_handler->getNumLaneToPreferredLane(current_lane); - - if (num != 0) { - return false; - } -#else const auto avoid_data = avoidance_data_; -#endif updateInfoMarker(avoid_data); updateDebugMarker(avoid_data, path_shifter_, debug_data_); -#ifndef USE_OLD_ARCHITECTURE // there is object that should be avoid. return true. if (!!avoid_data.stop_target_object) { return true; @@ -150,7 +127,6 @@ bool AvoidanceModule::isExecutionRequested() const if (avoid_data.unapproved_new_sl.empty()) { return false; } -#endif return !avoid_data.target_objects.empty(); } @@ -201,14 +177,10 @@ ModuleStatus AvoidanceModule::updateState() helper_.setPreviousDrivingLanes(data.current_lanelets); -#ifdef USE_OLD_ARCHITECTURE - return ModuleStatus::RUNNING; -#else if (is_plan_running || current_state_ == ModuleStatus::RUNNING) { return ModuleStatus::RUNNING; } return ModuleStatus::IDLE; -#endif } bool AvoidanceModule::isAvoidancePlanRunning() const @@ -253,14 +225,9 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb }(); // center line path (output of this function must have size > 1) -#ifdef USE_OLD_ARCHITECTURE - const auto center_path = - utils::calcCenterLinePath(planner_data_, reference_pose, longest_dist_to_shift_line); -#else const auto center_path = utils::calcCenterLinePath( planner_data_, reference_pose, longest_dist_to_shift_line, *getPreviousModuleOutput().reference_path); -#endif debug.center_line = center_path; if (center_path.points.size() < 2) { @@ -270,11 +237,7 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb } // reference path -#ifdef USE_OLD_ARCHITECTURE - data.reference_path_rough = center_path; -#else data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); -#endif data.reference_path = utils::resamplePathWithSpline( data.reference_path_rough, parameters_->resample_interval_for_planning); @@ -295,14 +258,8 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); // lanelet info -#ifdef USE_OLD_ARCHITECTURE - data.current_lanelets = utils::calcLaneAroundPose( - planner_data_->route_handler, reference_pose, planner_data_->parameters.forward_path_length, - planner_data_->parameters.backward_path_length); -#else data.current_lanelets = utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif // keep avoidance state data.state = avoidance_data_.state; @@ -2676,11 +2633,9 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); -#ifndef USE_OLD_ARCHITECTURE if (path_shifter_.getShiftLines().empty()) { out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } -#endif const auto all_unavoidable = std::all_of( data.target_objects.begin(), data.target_objects.end(), @@ -2932,7 +2887,6 @@ void AvoidanceModule::updateData() helper_.setData(planner_data_); -#ifndef USE_OLD_ARCHITECTURE if (!helper_.isInitialized()) { helper_.setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); helper_.setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); @@ -2940,7 +2894,6 @@ void AvoidanceModule::updateData() helper_.setPreviousDrivingLanes( utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_)); } -#endif debug_data_ = DebugData(); avoidance_data_ = calcAvoidancePlanningData(debug_data_); @@ -2959,15 +2912,6 @@ void AvoidanceModule::updateData() // update registered shift point for new reference path & remove past objects updateRegisteredRawShiftLines(); -#ifdef USE_OLD_ARCHITECTURE - if (!helper_.isInitialized()) { - helper_.setPreviousSplineShiftPath(toShiftedPath(avoidance_data_.reference_path)); - helper_.setPreviousLinearShiftPath(toShiftedPath(avoidance_data_.reference_path)); - helper_.setPreviousReferencePath(avoidance_data_.reference_path); - helper_.setPreviousDrivingLanes(avoidance_data_.current_lanelets); - } -#endif - fillShiftLine(avoidance_data_, debug_data_); fillEgoStatus(avoidance_data_, debug_data_); fillDebugData(avoidance_data_, debug_data_); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index a8a7f89c54021..262fbb55301ba 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -145,19 +145,11 @@ std::pair projectObstacleVelocityToTrajectory( } } // namespace -#ifdef USE_OLD_ARCHITECTURE -DynamicAvoidanceModule::DynamicAvoidanceModule( - const std::string & name, rclcpp::Node & node, - std::shared_ptr parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, - parameters_{std::move(parameters)} -#else DynamicAvoidanceModule::DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)} -#endif { } @@ -216,12 +208,6 @@ ModuleStatus DynamicAvoidanceModule::updateState() return ModuleStatus::SUCCESS; } -#ifndef USE_OLD_ARCHITECTURE - if (!isActivated() || isWaitingApproval()) { - return ModuleStatus::IDLE; - } -#endif - return ModuleStatus::RUNNING; } diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 09eeb0697c698..c7725b0ccf0f0 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -47,20 +47,12 @@ using tier4_autoware_utils::inverseTransformPose; namespace behavior_path_planner { -#ifdef USE_OLD_ARCHITECTURE -GoalPlannerModule::GoalPlannerModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters} -{ -#else GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} { -#endif LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); @@ -232,11 +224,9 @@ BehaviorModuleOutput GoalPlannerModule::run() current_state_ = ModuleStatus::RUNNING; updateOccupancyGrid(); -#ifndef USE_OLD_ARCHITECTURE if (!isActivated()) { return planWaitingApproval(); } -#endif return plan(); } @@ -490,22 +480,12 @@ void GoalPlannerModule::generateGoalCandidates() { const auto & route_handler = planner_data_->route_handler; -// with old architecture, module instance is not cleared when new route is received -// so need to reset status here. -#ifdef USE_OLD_ARCHITECTURE - // initialize when receiving new route - if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) { - // this process causes twice reset when receiving first route. - resetStatus(); - } - last_received_time_ = std::make_unique(route_handler->getRouteHeader().stamp); - -#else + // with old architecture, module instance is not cleared when new route is received + // so need to reset status here. // todo: move this check out of this function after old architecture is removed if (!goal_candidates_.empty()) { return; } -#endif // calculate goal candidates const Pose goal_pose = route_handler->getGoalPose(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 26051c4d78f3e..ab2c87c7e1ff9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -82,22 +82,14 @@ ModuleStatus LaneChangeInterface::updateState() } if (!module_type_->isValidPath()) { -#ifdef USE_OLD_ARCHITECTURE - return ModuleStatus::FAILURE; -#else return ModuleStatus::RUNNING; -#endif } if (module_type_->isAbortState()) { -#ifdef USE_OLD_ARCHITECTURE - return module_type_->hasFinishedAbort() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING; -#else if (module_type_->hasFinishedAbort()) { resetLaneChangeModule(); } return ModuleStatus::RUNNING; -#endif } if (module_type_->hasFinishedLaneChange()) { @@ -153,14 +145,10 @@ ModuleStatus LaneChangeInterface::updateState() getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe. Cancel lane change."); module_type_->toCancelState(); -#ifdef USE_OLD_ARCHITECTURE - return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::FAILURE; -#else if (!isWaitingApproval()) { resetLaneChangeModule(); } return ModuleStatus::RUNNING; -#endif } if (!module_type_->isAbortEnabled()) { 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 deleted file mode 100644 index 2c0985251b845..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2021-2023 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. - -#include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" - -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" - -#include - -#include -#include - -namespace behavior_path_planner -{ -LaneFollowingModule::LaneFollowingModule(const std::string & name, rclcpp::Node & node) -// RTCInterface is temporarily registered, but not used. -: SceneModuleInterface{name, node, {}} -{ - initParam(); -} - -void LaneFollowingModule::initParam() -{ - clearWaitingApproval(); // no need approval -} - -bool LaneFollowingModule::isExecutionRequested() const -{ - return true; -} - -bool LaneFollowingModule::isExecutionReady() const -{ - return true; -} - -BT::NodeStatus LaneFollowingModule::updateState() -{ - return BT::NodeStatus::SUCCESS; -} - -BehaviorModuleOutput LaneFollowingModule::plan() -{ - return getReferencePath(); -} -CandidateOutput LaneFollowingModule::planCandidate() const -{ - const auto path = getReferencePath().path; - if (!path) { - return {}; - } - return CandidateOutput(*path); -} -void LaneFollowingModule::processOnEntry() -{ - initParam(); - current_state_ = BT::NodeStatus::RUNNING; -} -void LaneFollowingModule::processOnExit() -{ - initParam(); - current_state_ = BT::NodeStatus::SUCCESS; -} - -BehaviorModuleOutput LaneFollowingModule::getReferencePath() const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_odometry->pose.pose; - const double dist_threshold = planner_data_->parameters.ego_nearest_dist_threshold; - const double yaw_threshold = planner_data_->parameters.ego_nearest_yaw_threshold; - - lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithConstrainsWithinRoute( - current_pose, ¤t_lane, dist_threshold, yaw_threshold)) { - RCLCPP_ERROR_THROTTLE( - getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!"); - return {}; // TODO(Horibe) - } - - return utils::getReferencePath(current_lane, planner_data_); -} -} // namespace behavior_path_planner 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 deleted file mode 100644 index 855e099c1ce21..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp +++ /dev/null @@ -1,138 +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. - -#include "behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp" - -#include -#include - -namespace behavior_path_planner -{ -BT::NodeStatus isExecutionRequested( - const std::shared_ptr p, - const std::shared_ptr & status) -{ - const auto ret = p->isExecutionRequested(); - status->is_requested = 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, - const std::shared_ptr & module_status) -: BT::CoroActionNode(name, config), scene_module_(scene_module), module_status_(module_status) -{ -} - -BT::NodeStatus SceneModuleBTNodeInterface::tick() -{ - RCLCPP_DEBUG_STREAM( - scene_module_->getLogger(), "bt::tick is called. module name: " << scene_module_->name()); - auto current_status = BT::NodeStatus::RUNNING; - - scene_module_->onEntry(); - module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); - module_status_->is_execution_ready = scene_module_->isExecutionReady(); - - const bool is_lane_following = scene_module_->name() == "LaneFollowing"; - - const bool is_waiting_approval = !scene_module_->isActivated(); - if (is_waiting_approval && !is_lane_following) { - scene_module_->lockRTCCommand(); - 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. - scene_module_->updateData(); - auto res = setOutput("output", scene_module_->planWaitingApproval()); - if (!res) { - RCLCPP_ERROR_STREAM(scene_module_->getLogger(), "setOutput() failed : " << res.error()); - } - module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); - module_status_->is_execution_ready = scene_module_->isExecutionReady(); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - scene_module_->getLogger(), "behavior module has failed with exception: " << e.what()); - // std::exit(EXIT_FAILURE); // TODO(Horibe) do appropriate handing - } - scene_module_->unlockRTCCommand(); - return scene_module_->getNodeStatusWhileWaitingApproval(); - } - - while (rclcpp::ok()) { - // 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. - scene_module_->lockRTCCommand(); - try { - auto res = setOutput("output", scene_module_->run()); - if (!res) { - RCLCPP_ERROR_STREAM(scene_module_->getLogger(), "setOutput() failed : " << res.error()); - } - - scene_module_->updateCurrentState(); - current_status = scene_module_->getCurrentStatus(); - - // for data output - module_status_->status = current_status; - module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); - module_status_->is_execution_ready = scene_module_->isExecutionReady(); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - scene_module_->getLogger(), "behavior module has failed with exception: " << e.what()); - // std::exit(EXIT_FAILURE); // TODO(Horibe) do appropriate handing - } - - RCLCPP_DEBUG_STREAM( - scene_module_->getLogger(), "on tick: current status = " << BT::toStr(current_status)); - if (current_status != BT::NodeStatus::RUNNING) { - RCLCPP_DEBUG(scene_module_->getLogger(), "on tick: module ended."); - break; - } - - scene_module_->unlockRTCCommand(); - setStatusRunningAndYield(); - } - - scene_module_->onExit(); - RCLCPP_DEBUG_STREAM( - scene_module_->getLogger(), "on tick: return current status = " << BT::toStr(current_status)); - - return current_status; -} - -void SceneModuleBTNodeInterface::halt() -{ - scene_module_->onExit(); - BT::CoroActionNode::halt(); -} - -BT::NodeStatus SceneModuleBTNodeInterface::planCandidate(BT::TreeNode & self) -{ - RCLCPP_DEBUG_STREAM( - scene_module_->getLogger(), "bt::planCandidate module name: " << scene_module_->name()); - auto res = self.setOutput("output", scene_module_->planWaitingApproval()); - if (!res) { - RCLCPP_ERROR_STREAM(scene_module_->getLogger(), "setOutput() failed : " << res.error()); - } - - return BT::NodeStatus::SUCCESS; -} - -BT::PortsList SceneModuleBTNodeInterface::providedPorts() -{ - return {BT::OutputPort("output", "desc")}; -} - -} // namespace behavior_path_planner 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 fe2a03460b013..d235cd1866181 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 @@ -35,23 +35,12 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::getPoint; -#ifdef USE_OLD_ARCHITECTURE -SideShiftModule::SideShiftModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters) -: SceneModuleInterface{name, node, {}}, parameters_{parameters} -{ - using std::placeholders::_1; - lateral_offset_subscriber_ = node.create_subscription( - "~/input/lateral_offset", 1, std::bind(&SideShiftModule::onLateralOffset, this, _1)); -#else SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} { -#endif // If lateral offset is subscribed, it approves side shift module automatically clearWaitingApproval(); } @@ -186,7 +175,6 @@ ModuleStatus SideShiftModule::updateState() void SideShiftModule::updateData() { -#ifndef USE_OLD_ARCHITECTURE if ( planner_data_->lateral_offset != nullptr && planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) { @@ -196,7 +184,6 @@ void SideShiftModule::updateData() latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp; } } -#endif if (current_state_ != ModuleStatus::RUNNING) { return; @@ -214,10 +201,6 @@ void SideShiftModule::updateData() const auto reference_pose = prev_output_.shift_length.empty() ? planner_data_->self_odometry->pose.pose : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); -#ifdef USE_OLD_ARCHITECTURE - const auto centerline_path = - utils::calcCenterLinePath(planner_data_, reference_pose, longest_dist_to_shift_line); -#else if (prev_reference_.points.empty()) { prev_reference_ = *getPreviousModuleOutput().path; } @@ -227,15 +210,10 @@ void SideShiftModule::updateData() const auto centerline_path = utils::calcCenterLinePath( planner_data_, reference_pose, longest_dist_to_shift_line, *getPreviousModuleOutput().reference_path); -#endif constexpr double resample_interval = 1.0; -#ifdef USE_OLD_ARCHITECTURE - reference_path_ = utils::resamplePathWithSpline(centerline_path, resample_interval); -#else const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path); reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval); -#endif path_shifter_.setPath(reference_path_); @@ -359,32 +337,6 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() return output; } -#ifdef USE_OLD_ARCHITECTURE -void SideShiftModule::onLateralOffset(const LateralOffset::ConstSharedPtr lateral_offset_msg) -{ - const double new_lateral_offset = lateral_offset_msg->lateral_offset; - - RCLCPP_DEBUG( - getLogger(), "onLateralOffset start : lateral offset current = %f, new = &%f", - requested_lateral_offset_, new_lateral_offset); - - // offset is not changed. - if (std::abs(inserted_lateral_offset_ - new_lateral_offset) < 1e-4) { - return; - } - - if (parameters_->shift_request_time_limit < parameters_->time_to_start_shifting) { - RCLCPP_DEBUG( - getLogger(), "Shift request time might be too low. Generated trajectory might be wavy"); - } - // new offset is requested. - if (isReadyForNextRequest(parameters_->shift_request_time_limit)) { - lateral_offset_change_request_ = true; - requested_lateral_offset_ = new_lateral_offset; - } -} -#endif - ShiftLine SideShiftModule::calcShiftLine() const { const auto & p = parameters_; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index b2cf74893666b..533076be8012a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -36,30 +36,6 @@ using tier4_autoware_utils::inverseTransformPoint; namespace behavior_path_planner { -#ifdef USE_OLD_ARCHITECTURE -StartPlannerModule::StartPlannerModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, - parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} -{ - lane_departure_checker_ = std::make_shared(); - lane_departure_checker_->setVehicleInfo(vehicle_info_); - - // set enabled planner - if (parameters_->enable_shift_pull_out) { - start_planner_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_)); - } - if (parameters_->enable_geometric_pull_out) { - start_planner_planners_.push_back(std::make_shared(node, *parameters)); - } - if (start_planner_planners_.empty()) { - RCLCPP_ERROR(getLogger(), "Not found enabled planner"); - } -} -#else StartPlannerModule::StartPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -83,19 +59,12 @@ StartPlannerModule::StartPlannerModule( RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } } -#endif BehaviorModuleOutput StartPlannerModule::run() { -#ifdef USE_OLD_ARCHITECTURE - current_state_ = ModuleStatus::RUNNING; -#endif - -#ifndef USE_OLD_ARCHITECTURE if (!isActivated()) { return planWaitingApproval(); } -#endif return plan(); } @@ -115,9 +84,6 @@ bool StartPlannerModule::isExecutionRequested() const if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { -#ifdef USE_OLD_ARCHITECTURE - is_executed_ = false; -#endif return false; } @@ -125,29 +91,17 @@ bool StartPlannerModule::isExecutionRequested() const !planner_data_->prev_route_id || *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); -#ifdef USE_OLD_ARCHITECTURE - if (is_executed_) { - return true; - } -#endif - if (current_state_ == ModuleStatus::RUNNING) { return true; } if (!has_received_new_route_) { -#ifdef USE_OLD_ARCHITECTURE - is_executed_ = false; -#endif return false; } const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) < parameters_->th_stopped_velocity; if (!is_stopped) { -#ifdef USE_OLD_ARCHITECTURE - is_executed_ = false; -#endif return false; } @@ -163,15 +117,9 @@ bool StartPlannerModule::isExecutionRequested() const auto lanes = current_lanes; lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { -#ifdef USE_OLD_ARCHITECTURE - is_executed_ = false; -#endif return false; } -#ifdef USE_OLD_ARCHITECTURE - is_executed_ = true; -#endif return true; } @@ -184,17 +132,11 @@ ModuleStatus StartPlannerModule::updateState() { RCLCPP_DEBUG(getLogger(), "START_PLANNER updateState"); -#ifdef USE_OLD_ARCHITECTURE - if (isActivated() && !isWaitingApproval()) { - current_state_ = ModuleStatus::RUNNING; - } -#else if (isActivated() && !isWaitingApproval()) { current_state_ = ModuleStatus::RUNNING; } else { current_state_ = ModuleStatus::IDLE; } -#endif if (hasFinishedPullOut()) { return ModuleStatus::SUCCESS; @@ -729,10 +671,6 @@ bool StartPlannerModule::hasFinishedPullOut() const const bool has_finished = arclength_current.length - arclength_pull_out_end.length > 0.0; -#ifdef USE_OLD_ARCHITECTURE - is_executed_ = !has_finished; -#endif - return has_finished; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 085f72b59d31e..5d77027efa5d0 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -30,19 +30,8 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { BehaviorModuleOutput output = -#ifdef USE_OLD_ARCHITECTURE - // generate reference path in this planner - std::invoke([this, &planner_data]() { - auto path = getReferencePath(planner_data); - if (!path) { - return BehaviorModuleOutput{}; - } - return *path; - }); -#else // use planner previous module reference path getPreviousModuleOutput(); -#endif const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(*(output.path), planner_data); output.path = std::make_shared(smoothed_path); @@ -50,29 +39,6 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( return output; } -#ifdef USE_OLD_ARCHITECTURE -boost::optional DefaultFixedGoalPlanner::getReferencePath( - const std::shared_ptr & planner_data) const -{ - const auto & route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_odometry->pose.pose; - const double dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; - const double yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; - - lanelet::ConstLanelet current_lane{}; - if (route_handler->getClosestLaneletWithConstrainsWithinRoute( - current_pose, ¤t_lane, dist_threshold, yaw_threshold)) { - return utils::getReferencePath(current_lane, planner_data); - } - - if (route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { - return utils::getReferencePath(current_lane, planner_data); - } - - return {}; // something wrong -} -#endif - PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( const PathWithLaneId & path, const std::shared_ptr & planner_data) const { diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 5b89b352d9574..6e3a2dc5fa00e 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2409,25 +2409,6 @@ PathWithLaneId getCenterLinePath( const double s_backward = std::max(0., s - backward_path_length); double s_forward = s + forward_path_length; -#ifdef USE_OLD_ARCHITECTURE - const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(lanelet_sequence.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(parameter, shift_intervals, 0.0); - - if (route_handler.isDeadEndLanelet(lanelet_sequence.back())) { - const double forward_length = std::max(lane_length - lane_change_buffer, 0.0); - s_forward = std::min(s_forward, forward_length); - } - - if (route_handler.isInGoalRouteSection(lanelet_sequence.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence, route_handler.getGoalPose()); - const double forward_length = std::max(goal_arc_coordinates.length - lane_change_buffer, 0.0); - s_forward = std::min(s_forward, forward_length); - } -#else if (route_handler.isDeadEndLanelet(lanelet_sequence.back())) { const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); s_forward = std::clamp(s_forward, 0.0, lane_length); @@ -2438,7 +2419,6 @@ PathWithLaneId getCenterLinePath( lanelet::utils::getArcCoordinates(lanelet_sequence, route_handler.getGoalPose()); s_forward = std::clamp(s_forward, 0.0, goal_arc_coordinates.length); } -#endif const auto raw_path_with_lane_id = route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true); @@ -2895,10 +2875,8 @@ DrivableAreaInfo combineDrivableAreaInfo( DrivableAreaInfo combined_drivable_area_info; // drivable lanes -#ifndef USE_OLD_ARCHITECTURE combined_drivable_area_info.drivable_lanes = combineDrivableLanes(drivable_area_info1.drivable_lanes, drivable_area_info2.drivable_lanes); -#endif // obstacles for (const auto & obstacle : drivable_area_info1.obstacles) {