From 87c2bb03bd5abfe3237add983eabfa27c4410284 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 22 Jul 2022 11:26:25 +0900 Subject: [PATCH] fix(behavior_path_planner): remove unnecessary publisher and subscriber (#1371) * fix(behavior_path_planner): remove unnecessary publisher and subscriber Signed-off-by: Fumiya Watanabe * fix(tier4_planning_launch): fix launch file Signed-off-by: Fumiya Watanabe * fix(tier4_planning_launch): fix xml file Signed-off-by: Fumiya Watanabe --- .../behavior_planning.launch.py | 39 ------------------- .../behavior_planning.launch.xml | 10 ----- .../behavior_path_planner_node.hpp | 9 ----- .../src/behavior_path_planner_node.cpp | 13 ------- 4 files changed, 71 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 8b917ccbc2f3e..9a18fa1e9208a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -17,7 +17,6 @@ from ament_index_python.packages import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess from launch.actions import GroupAction from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction @@ -142,32 +141,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/perception", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/scenario", "/planning/scenario_planning/scenario"), - ( - "~/input/external_approval", - "/planning/scenario_planning/lane_driving/behavior_planning/" - "behavior_path_planner/path_change_approval", - ), - ( - "~/input/force_approval", - "/planning/scenario_planning/lane_driving/behavior_planning/" - "behavior_path_planner/path_change_force", - ), ("~/output/path", "path_with_lane_id"), - ( - "~/output/ready", - "/planning/scenario_planning/lane_driving/behavior_planning/" - "behavior_path_planner/ready_module", - ), - ( - "~/output/running", - "/planning/scenario_planning/lane_driving/behavior_planning/" - "behavior_path_planner/running_modules", - ), - ( - "~/output/force_available", - "/planning/scenario_planning/lane_driving/behavior_planning/" - "behavior_path_planner/force_available", - ), ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"), ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"), ], @@ -465,19 +439,6 @@ def launch_setup(context, *args, **kwargs): [ container, load_compare_map, - ExecuteProcess( - cmd=[ - "ros2", - "topic", - "pub", - "/planning/scenario_planning/lane_driving/behavior_planning/" - "behavior_path_planner/path_change_approval", - "tier4_planning_msgs/msg/Approval", - "{approval: true}", - "-r", - "10", - ] - ), ] ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 9b12e1393f95b..86db8c4333972 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -3,22 +3,12 @@ - - - - - - - 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 a4d89bbdbcc92..fda89c0c416fb 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 @@ -76,13 +76,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; - rclcpp::Subscription::SharedPtr external_approval_subscriber_; - rclcpp::Subscription::SharedPtr force_approval_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; rclcpp::Publisher::SharedPtr path_candidate_publisher_; - rclcpp::Publisher::SharedPtr force_available_publisher_; - rclcpp::Publisher::SharedPtr plan_ready_publisher_; - rclcpp::Publisher::SharedPtr plan_running_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -92,10 +87,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; Scenario::SharedPtr current_scenario_{nullptr}; - std::string prev_ready_module_name_ = "NONE"; - PathChangeModule ready_module_{}; - PathChangeModuleArray running_modules_{}; - TurnSignalDecider turn_signal_decider_; std::mutex mutex_pd_; // mutex for planner_data_ 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 c1f8b559a3235..f675357cd014d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -70,12 +70,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); - // For remote operation - plan_ready_publisher_ = create_publisher("~/output/ready", 1); - plan_running_publisher_ = create_publisher("~/output/running", 1); - force_available_publisher_ = - create_publisher("~/output/force_available", 1); - // Debug debug_marker_publisher_ = create_publisher("~/debug/markers", 1); @@ -102,13 +96,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod current_scenario_ = std::make_shared(*msg); }, createSubscriptionOptions(this)); - external_approval_subscriber_ = create_subscription( - "~/input/external_approval", 1, - std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1), - createSubscriptionOptions(this)); - force_approval_subscriber_ = create_subscription( - "~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1), - createSubscriptionOptions(this)); // route_handler auto qos_transient_local = rclcpp::QoS{1}.transient_local();