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 d95175853689f..3248b955d1a6a 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 @@ -24,13 +24,13 @@ #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" +#include "planning_manager/msg/planning_data.hpp" #include "planning_manager/srv/behavior_path_planner_plan.hpp" #include "planning_manager/srv/behavior_path_planner_validate.hpp" #include #include -#include "planning_manager/msg/planning_data.hpp" #include #include #include 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 1c6e3de5fa976..7667e70c945a3 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -622,7 +622,8 @@ void BehaviorPathPlannerNode::setPlanningData([[maybe_unused]] const PlanningDat planner_data_->route_handler->setMap(planning_data.had_map_bin); planner_data_->self_odometry = std::make_shared(planning_data.ego_odom); - planner_data_->dynamic_object = std::make_shared(planning_data.predicted_objects); + planner_data_->dynamic_object = + std::make_shared(planning_data.predicted_objects); // external approval planner_data_->approval.is_approved.data = planning_data.external_approval.approval; @@ -637,7 +638,8 @@ void BehaviorPathPlannerNode::setPlanningData([[maybe_unused]] const PlanningDat return "NONE"; } }; - planner_data_->approval.is_force_approved.module_name = getModuleName(planning_data.force_approval.module); + planner_data_->approval.is_force_approved.module_name = + getModuleName(planning_data.force_approval.module); planner_data_->approval.is_force_approved.stamp = planning_data.header.stamp; } diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp index 84c7c383bff2b..7f659000bc358 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp @@ -17,12 +17,12 @@ #include "behavior_velocity_planner/planner_data.hpp" #include "behavior_velocity_planner/planner_manager.hpp" +#include "planning_manager/msg/planning_data.hpp" #include "planning_manager/srv/behavior_velocity_planner_plan.hpp" #include "planning_manager/srv/behavior_velocity_planner_validate.hpp" #include -#include "planning_manager/msg/planning_data.hpp" #include #include #include @@ -100,8 +100,10 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Service::SharedPtr srv_planning_manager_plan_; - rclcpp::Service::SharedPtr srv_planning_manager_validate_; + rclcpp::Service::SharedPtr + srv_planning_manager_plan_; + rclcpp::Service::SharedPtr + srv_planning_manager_validate_; void publishDebugMarker(const autoware_auto_planning_msgs::msg::Path & path); diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 5b2be6574fb54..dfc474299b802 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -15,7 +15,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs diagnostic_msgs - planning_manager eigen geometry_msgs grid_map_ros @@ -26,6 +25,7 @@ nav_msgs nlohmann-json-dev pcl_conversions + planning_manager rclcpp rclcpp_components sensor_msgs diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 8a96f9f52a058..f2bfcf1abf716 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -159,9 +159,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio srv_planning_manager_plan_ = create_service( "~/srv/planning_manager/plan", std::bind(&BehaviorVelocityPlannerNode::onPlanService, this, _1, _2)); - srv_planning_manager_validate_ = create_service( - "~/srv/planning_manager/validate", - std::bind(&BehaviorVelocityPlannerNode::onValidateService, this, _1, _2)); + srv_planning_manager_validate_ = + create_service( + "~/srv/planning_manager/validate", + std::bind(&BehaviorVelocityPlannerNode::onValidateService, this, _1, _2)); } bool BehaviorVelocityPlannerNode::isDataReady() @@ -379,7 +380,8 @@ void BehaviorVelocityPlannerNode::onValidateService( response->result.data = true; } -void BehaviorVelocityPlannerNode::setPlanningData([[maybe_unused]] const planning_manager::msg::PlanningData planning_data) +void BehaviorVelocityPlannerNode::setPlanningData( + [[maybe_unused]] const planning_manager::msg::PlanningData planning_data) { } diff --git a/planning/planning_manager/include/planning_manager/planning_manager_node.hpp b/planning/planning_manager/include/planning_manager/planning_manager_node.hpp index 73e4aa49b2892..886c3f3afe61d 100644 --- a/planning/planning_manager/include/planning_manager/planning_manager_node.hpp +++ b/planning/planning_manager/include/planning_manager/planning_manager_node.hpp @@ -26,7 +26,6 @@ #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_auto_planning_msgs/msg/planning_data.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" - #include #include #include @@ -36,21 +35,21 @@ namespace planning_manager { using autoware_auto_planning_msgs::msg::HADMapRoute; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; +using planning_manager::msg::PlanningData; using planning_manager::srv::BehaviorPathPlannerPlan; using planning_manager::srv::BehaviorPathPlannerValidate; using planning_manager::srv::BehaviorVelocityPlannerPlan; using planning_manager::srv::BehaviorVelocityPlannerValidate; -using planning_manager::msg::PlanningData; // MEMO(murooka) beahvior path -using tier4_planning_msgs::msg::Approval; using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::HADMapRoute; using nav_msgs::msg::Odometry; +using tier4_planning_msgs::msg::Approval; using tier4_planning_msgs::msg::PathChangeModule; class PlanningManagerNode : public rclcpp::Node @@ -87,12 +86,9 @@ class PlanningManagerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_services_; // clients - rclcpp::Client::SharedPtr - client_behavior_path_planner_plan_; - rclcpp::Client::SharedPtr - client_behavior_path_planner_validate_; - rclcpp::Client::SharedPtr - client_behavior_velocity_planner_plan_; + rclcpp::Client::SharedPtr client_behavior_path_planner_plan_; + rclcpp::Client::SharedPtr client_behavior_path_planner_validate_; + rclcpp::Client::SharedPtr client_behavior_velocity_planner_plan_; rclcpp::Client::SharedPtr client_behavior_velocity_planner_validate_; diff --git a/planning/planning_manager/package.xml b/planning/planning_manager/package.xml index 14e47f31cc8a5..bd32e1022eaee 100644 --- a/planning/planning_manager/package.xml +++ b/planning/planning_manager/package.xml @@ -14,11 +14,11 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + nav_msgs rclcpp std_msgs tier4_autoware_utils tier4_planning_msgs - nav_msgs ament_lint_auto autoware_lint_common diff --git a/planning/planning_manager/src/planning_manager_node.cpp b/planning/planning_manager/src/planning_manager_node.cpp index ae8f40b6e09f4..e439288d95159 100644 --- a/planning/planning_manager/src/planning_manager_node.cpp +++ b/planning/planning_manager/src/planning_manager_node.cpp @@ -73,11 +73,12 @@ PlanningManagerNode::PlanningManagerNode(const rclcpp::NodeOptions & node_option std::bind(&PlanningManagerNode::onMap, this, _1)); odom_sub_ = create_subscription( "~/input/odometry", 1, std::bind(&PlanningManagerNode::onOdometry, this, _1)); - predicted_objects_sub_ = create_subscription( - "~/input/predicted_objects", 1, std::bind(&PlanningManagerNode::onPredictedObjects, this, _1)); + predicted_objects_sub_ = + create_subscription( + "~/input/predicted_objects", 1, + std::bind(&PlanningManagerNode::onPredictedObjects, this, _1)); external_approval_sub_ = create_subscription( - "~/input/external_approval", 1, - std::bind(&PlanningManagerNode::onExternalApproval, this, _1)); + "~/input/external_approval", 1, std::bind(&PlanningManagerNode::onExternalApproval, this, _1)); force_approval_sub_ = create_subscription( "~/input/force_approval", 1, std::bind(&PlanningManagerNode::onForceApproval, this, _1)); @@ -256,10 +257,12 @@ void PlanningManagerNode::validateTrajectory([[maybe_unused]] const Trajectory & client_behavior_velocity_planner_validate_, behavior_velocity_request); } -void PlanningManagerNode::publishTrajectory(const Trajectory & traj) { +void PlanningManagerNode::publishTrajectory(const Trajectory & traj) +{ // TODO(murooka) use thread local for member variable? RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start publishTrajectory"); - traj_pub_->publish(traj); } + traj_pub_->publish(traj); +} void PlanningManagerNode::publishDiagnostics() {} } // namespace planning_manager