From 2340729a8eb182fa76b76ff06465e6211bba64c0 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 6 Nov 2023 02:45:27 +0000 Subject: [PATCH 1/8] split up classes in planner_node.hpp into 3 files --- soccer/src/soccer/CMakeLists.txt | 1 + soccer/src/soccer/planning/global_state.hpp | 97 +++++++ .../src/soccer/planning/planner_for_robot.cpp | 267 ++++++++++++++++++ .../src/soccer/planning/planner_for_robot.hpp | 145 ++++++++++ soccer/src/soccer/planning/planner_node.cpp | 261 ----------------- soccer/src/soccer/planning/planner_node.hpp | 230 +-------------- 6 files changed, 511 insertions(+), 490 deletions(-) create mode 100644 soccer/src/soccer/planning/global_state.hpp create mode 100644 soccer/src/soccer/planning/planner_for_robot.cpp create mode 100644 soccer/src/soccer/planning/planner_for_robot.hpp diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 813a63ad3ee..a4ea283380d 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -42,6 +42,7 @@ set(ROBOCUP_LIB_SRC planning/planner/settle_path_planner.cpp planning/planner/goalie_idle_path_planner.cpp planning/planner_node.cpp + planning/planner_for_robot.cpp planning/trajectory.cpp planning/trajectory_utils.cpp planning/trajectory_collection.cpp diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp new file mode 100644 index 00000000000..906e1234485 --- /dev/null +++ b/soccer/src/soccer/planning/global_state.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include +#include "planning/planner/escape_obstacles_path_planner.hpp" + +namespace planning { + +/** + * Aggregates info from other ROS nodes into an unchanging "global state" for + * each planner. Read-only (from PlannerNode's perspective). + * + * ("Global state" in quotes since many of these fields can be changed by other + * nodes; however, to PlannerNode these are immutable.) + */ +class GlobalState { +public: + GlobalState(rclcpp::Node* node) { + play_state_sub_ = node->create_subscription( + referee::topics::kPlayStateTopic, rclcpp::QoS(1), + [this](rj_msgs::msg::PlayState::SharedPtr state) { // NOLINT + last_play_state_ = rj_convert::convert_from_ros(*state); + }); + game_settings_sub_ = node->create_subscription( + config_server::topics::kGameSettingsTopic, rclcpp::QoS(1), + [this](rj_msgs::msg::GameSettings::SharedPtr settings) { // NOLINT + last_game_settings_ = rj_convert::convert_from_ros(*settings); + }); + goalie_sub_ = node->create_subscription( + referee::topics::kGoalieTopic, rclcpp::QoS(1), + [this](rj_msgs::msg::Goalie::SharedPtr goalie) { // NOLINT + last_goalie_id_ = goalie->goalie_id; + }); + global_obstacles_sub_ = node->create_subscription( + planning::topics::kGlobalObstaclesTopic, rclcpp::QoS(1), + [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr global_obstacles) { // NOLINT + last_global_obstacles_ = rj_convert::convert_from_ros(*global_obstacles); + }); + def_area_obstacles_sub_ = node->create_subscription( + planning::topics::kDefAreaObstaclesTopic, rclcpp::QoS(1), + [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr def_area_obstacles) { // NOLINT + last_def_area_obstacles_ = rj_convert::convert_from_ros(*def_area_obstacles); + }); + world_state_sub_ = node->create_subscription( + vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), + [this](rj_msgs::msg::WorldState::SharedPtr world_state) { // NOLINT + last_world_state_ = rj_convert::convert_from_ros(*world_state); + }); + coach_state_sub_ = node->create_subscription( + "/strategy/coach_state", rclcpp::QoS(1), + [this](rj_msgs::msg::CoachState::SharedPtr coach_state) { // NOLINT + last_coach_state_ = *coach_state; + }); + } + + [[nodiscard]] PlayState play_state() const { + return last_play_state_; + } + [[nodiscard]] GameSettings game_settings() const { + return last_game_settings_; + } + [[nodiscard]] int goalie_id() const { + return last_goalie_id_; + } + [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const { + return last_global_obstacles_; + } + [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const { + return last_def_area_obstacles_; + } + [[nodiscard]] const WorldState* world_state() const { + return &last_world_state_; + } + [[nodiscard]] const rj_msgs::msg::CoachState coach_state() const { + return last_coach_state_; + } + +private: + rclcpp::Subscription::SharedPtr play_state_sub_; + rclcpp::Subscription::SharedPtr game_settings_sub_; + rclcpp::Subscription::SharedPtr goalie_sub_; + rclcpp::Subscription::SharedPtr global_obstacles_sub_; + rclcpp::Subscription::SharedPtr def_area_obstacles_sub_; + rclcpp::Subscription::SharedPtr world_state_sub_; + rclcpp::Subscription::SharedPtr coach_state_sub_; + + PlayState last_play_state_ = PlayState::halt(); + GameSettings last_game_settings_; + int last_goalie_id_; + rj_geometry::ShapeSet last_global_obstacles_; + rj_geometry::ShapeSet last_def_area_obstacles_; + WorldState last_world_state_; + rj_msgs::msg::CoachState last_coach_state_; +}; + +} // namespace planning diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp new file mode 100644 index 00000000000..467d9005b97 --- /dev/null +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -0,0 +1,267 @@ +#include "planner_for_robot.hpp" + +#include "planning/planner/collect_path_planner.hpp" +#include "planning/planner/escape_obstacles_path_planner.hpp" +#include "planning/planner/goalie_idle_path_planner.hpp" +#include "planning/planner/intercept_path_planner.hpp" +#include "planning/planner/line_kick_path_planner.hpp" +#include "planning/planner/path_target_path_planner.hpp" +#include "planning/planner/pivot_path_planner.hpp" +#include "planning/planner/settle_path_planner.hpp" + +namespace planning { + +PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node, + TrajectoryCollection* robot_trajectories, + const GlobalState& global_state) + : node_{node}, + robot_id_{robot_id}, + robot_trajectories_{robot_trajectories}, + global_state_{global_state}, + debug_draw_{ + node->create_publisher(viz::topics::kDebugDrawTopic, 10), + fmt::format("planning_{}", robot_id)} { + // create map of {planner name -> planner} + path_planners_[GoalieIdlePathPlanner().name()] = std::make_unique(); + path_planners_[InterceptPathPlanner().name()] = std::make_unique(); + path_planners_[PathTargetPathPlanner().name()] = std::make_unique(); + path_planners_[SettlePathPlanner().name()] = std::make_unique(); + path_planners_[CollectPathPlanner().name()] = std::make_unique(); + path_planners_[LineKickPathPlanner().name()] = std::make_unique(); + path_planners_[PivotPathPlanner().name()] = std::make_unique(); + path_planners_[EscapeObstaclesPathPlanner().name()] = + std::make_unique(); + + // publish paths to control + trajectory_topic_ = node_->create_publisher( + planning::topics::trajectory_topic(robot_id), rclcpp::QoS(1).transient_local()); + + // publish kicker/dribbler cmds directly to radio + manipulator_pub_ = node->create_publisher( + control::topics::manipulator_setpoint_topic(robot_id), rclcpp::QoS(10)); + + // for ball sense and possession + robot_status_sub_ = node_->create_subscription( + radio::topics::robot_status_topic(robot_id), rclcpp::QoS(1), + [this](rj_msgs::msg::RobotStatus::SharedPtr status) { // NOLINT + had_break_beam_ = status->has_ball_sense; + }); + + // For hypothetical path planning + hypothetical_path_service_ = node_->create_service( + fmt::format("hypothetical_trajectory_robot_{}", robot_id), + [this](const std::shared_ptr request, + std::shared_ptr response) { + plan_hypothetical_robot_path(request, response); + }); +} + +void PlannerForRobot::execute_intent(const RobotIntent& intent) { + if (robot_alive()) { + // plan a path and send it to control + auto plan_request = make_request(intent); + + auto trajectory = safe_plan_for_robot(plan_request); + trajectory_topic_->publish(rj_convert::convert_to_ros(trajectory)); + + // send the kick/dribble commands to the radio + manipulator_pub_->publish(rj_msgs::build() + .shoot_mode(intent.shoot_mode) + .trigger_mode(intent.trigger_mode) + .kick_speed(intent.kick_speed) + .dribbler_speed(plan_request.dribbler_speed)); + + /* + // TODO (PR #1970): fix TrajectoryCollection + // store all latest trajectories in a mutex-locked shared map + robot_trajectories_->put(robot_id_, std::make_shared(std::move(trajectory)), + intent.priority); + */ + } +} + +void PlannerForRobot::plan_hypothetical_robot_path( + const std::shared_ptr& request, + std::shared_ptr& response) { + /* const auto intent = rj_convert::convert_from_ros(request->intent); */ + /* auto plan_request = make_request(intent); */ + /* auto trajectory = safe_plan_for_robot(plan_request); */ + /* RJ::Seconds trajectory_duration = trajectory.duration(); */ + /* response->estimate = rj_convert::convert_to_ros(trajectory_duration); */ +} + +std::optional PlannerForRobot::get_time_left() const { + // TODO(p-nayak): why does this say 3s even when the robot is on its point? + // get the Traj out of the relevant [Trajectory, priority] tuple in + // robot_trajectories_ + + /* + // TODO (PR #1970): fix TrajectoryCollection + const auto& [latest_traj, priority] = robot_trajectories_->get(robot_id_); + if (!latest_traj) { + return std::nullopt; + } + return latest_traj->end_time() - RJ::now(); + */ + return std::nullopt; +} + +PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { + // pass global_state_ directly + const auto* world_state = global_state_.world_state(); + const auto goalie_id = global_state_.goalie_id(); + const auto play_state = global_state_.play_state(); + const auto min_dist_from_ball = global_state_.coach_state().global_override.min_dist_from_ball; + const auto max_robot_speed = global_state_.coach_state().global_override.max_speed; + const auto max_dribbler_speed = global_state_.coach_state().global_override.max_dribbler_speed; + const auto& robot = world_state->our_robots.at(robot_id_); + const auto start = RobotInstant{robot.pose, robot.velocity, robot.timestamp}; + + const auto global_obstacles = global_state_.global_obstacles(); + rj_geometry::ShapeSet real_obstacles = global_obstacles; + + const auto def_area_obstacles = global_state_.def_area_obstacles(); + rj_geometry::ShapeSet virtual_obstacles = intent.local_obstacles; + const bool is_goalie = goalie_id == robot_id_; + if (!is_goalie) { + virtual_obstacles.add(def_area_obstacles); + } + + /* + // TODO (PR #1970): fix TrajectoryCollection + // make a copy instead of getting the actual shared_ptr to Trajectory + std::array, kNumShells> planned_trajectories; + + for (size_t i = 0; i < kNumShells; i++) { + // TODO(Kevin): check that priority works (seems like + // robot_trajectories_ is passed on init, when no planning has occured + // yet) + const auto& [trajectory, priority] = robot_trajectories_->get(i); + if (i != robot_id_ && priority >= intent.priority) { + if (!trajectory) { + planned_trajectories[i] = std::nullopt; + } else { + planned_trajectories[i] = std::make_optional(*trajectory.get()); + } + } + } + */ + + RobotConstraints constraints; + MotionCommand motion_command; + // Attempting to create trajectories with max speeds <= 0 crashes the planner (during RRT + // generation) + if (max_robot_speed == 0.0f) { + // If coach node has speed set to 0, + // force HALT by replacing the MotionCommand with an empty one. + motion_command = MotionCommand{}; + } else if (max_robot_speed < 0.0f) { + // If coach node has speed set to negative, assume infinity. + // Negative numbers cause crashes, but 10 m/s is an effectively infinite limit. + motion_command = intent.motion_command; + constraints.mot.max_speed = 10.0f; + } else { + motion_command = intent.motion_command; + constraints.mot.max_speed = max_robot_speed; + } + + float dribble_speed = + std::min(static_cast(intent.dribbler_speed), static_cast(max_dribbler_speed)); + + return PlanRequest{start, + motion_command, + constraints, + std::move(real_obstacles), + std::move(virtual_obstacles), + robot_trajectories_, + static_cast(robot_id_), + world_state, + intent.priority, + &debug_draw_, + had_break_beam_, + min_dist_from_ball, + dribble_speed}; +} + +Trajectory PlannerForRobot::unsafe_plan_for_robot(const planning::PlanRequest& request) { + if (path_planners_.count(request.motion_command.name) == 0) { + throw std::runtime_error(fmt::format("ID {}: MotionCommand name <{}> does not exist!", + robot_id_, request.motion_command.name)); + } + + // get Trajectory from the planner requested in MotionCommand + current_path_planner_ = path_planners_[request.motion_command.name].get(); + Trajectory trajectory = current_path_planner_->plan(request); + + if (trajectory.empty()) { + // empty Trajectory means current_path_planner_ has failed + // if current_path_planner_ fails, reset it before throwing exception + current_path_planner_->reset(); + throw std::runtime_error(fmt::format("PathPlanner <{}> failed to create valid Trajectory!", + current_path_planner_->name())); + } + + if (!trajectory.angles_valid()) { + throw std::runtime_error(fmt::format("Trajectory returned from <{}> has no angle profile!", + current_path_planner_->name())); + } + + if (!trajectory.time_created().has_value()) { + throw std::runtime_error(fmt::format("Trajectory returned from <{}> has no timestamp!", + current_path_planner_->name())); + } + + return trajectory; +} + +Trajectory PlannerForRobot::safe_plan_for_robot(const planning::PlanRequest& request) { + Trajectory trajectory; + try { + trajectory = unsafe_plan_for_robot(request); + } catch (std::runtime_error exception) { + SPDLOG_WARN("PlannerForRobot {} error caught: {}", robot_id_, exception.what()); + SPDLOG_WARN("PlannerForRobot {}: Defaulting to EscapeObstaclesPathPlanner", robot_id_); + + current_path_planner_ = default_path_planner_.get(); + trajectory = current_path_planner_->plan(request); + // TODO(Kevin): planning should be able to send empty Trajectory + // without crashing, instead of resorting to default planner + // (currently the ros_convert throws "cannot serialize trajectory with + // invalid angles") + } + + // draw robot's desired path + std::vector path; + std::transform(trajectory.instants().begin(), trajectory.instants().end(), + std::back_inserter(path), + [](const auto& instant) { return instant.position(); }); + debug_draw_.draw_path(path); + + // draw robot's desired endpoint + debug_draw_.draw_circle(rj_geometry::Circle(path.back(), kRobotRadius), Qt::black); + + // draw obstacles for this robot + // TODO: these will stack atop each other, since each robot draws obstacles + debug_draw_.draw_shapes(global_state_.global_obstacles(), QColor(255, 0, 0, 30)); + debug_draw_.draw_shapes(request.virtual_obstacles, QColor(255, 0, 0, 30)); + debug_draw_.publish(); + + return trajectory; +} + +bool PlannerForRobot::robot_alive() const { + return global_state_.world_state()->our_robots.at(robot_id_).visible && + RJ::now() < global_state_.world_state()->last_updated_time + RJ::Seconds(PARAM_timeout); +} + +bool PlannerForRobot::is_done() const { + // no segfaults + if (current_path_planner_ == nullptr) { + return false; + } + + return current_path_planner_->is_done(); +} + +} // namespace planning diff --git a/soccer/src/soccer/planning/planner_for_robot.hpp b/soccer/src/soccer/planning/planner_for_robot.hpp new file mode 100644 index 00000000000..ee45e2ff1e5 --- /dev/null +++ b/soccer/src/soccer/planning/planner_for_robot.hpp @@ -0,0 +1,145 @@ +#pragma once + +#include +#include +#include +#include "global_state.hpp" + +namespace planning { + +/** + * @brief Handles one robot's planning, which for us is a translation of RobotIntent to + * Trajectory. Bundles together all relevant ROS pub/subs and forwards + * RobotIntents to the appropriate PathPlanner (see path_planner.hpp). + * + * In general, prefers default behavior and WARN-level logs over crashing. + * + * (PlannerNode makes N PlannerForRobots and handles them all, see + * PlannerNode below.) + */ +class PlannerForRobot { +public: + PlannerForRobot(int robot_id, rclcpp::Node* node, TrajectoryCollection* robot_trajectories, + const GlobalState& global_state); + + PlannerForRobot(PlannerForRobot&&) = delete; + const PlannerForRobot& operator=(PlannerForRobot&&) = delete; + PlannerForRobot(const PlannerForRobot&) = delete; + const PlannerForRobot& operator=(const PlannerForRobot&) = delete; + + ~PlannerForRobot() = default; + + /** + * Entry point for the planner node's ActionServer. + * + * Creates and publishes a Trajectory based on the given RobotIntent. Also + * publishes a ManipulatorSetpoint to control kicker/dribbler/chipper. + */ + void execute_intent(const RobotIntent& intent); + + /* + * @brief estimate the amount of time it would take for a robot to execute a robot intent + * (SERVICE). + * + * @param request Requested RobotIntent resulting in the hypothetical robot path. + * @param response The response object that will contain the resultant time to completion of a + * hypothetical path. + */ + // TODO(Kevin): I broke this, sorry + void plan_hypothetical_robot_path( + const std::shared_ptr& request, + std::shared_ptr& response); + + /** + * @return RJ::Seconds time left for the trajectory to complete + * + * Note: RJ::Seconds is an alias for std::chrono::duration. + */ + [[nodiscard]] std::optional get_time_left() const; + + /* + * @return true if current planner is done, false otherwise. + */ + [[nodiscard]] bool is_done() const; + +private: + /** + * @brief Create a PlanRequest based on the given RobotIntent. + * + * @details This is how gameplay's RobotIntents end up in the right planner (e.g. + * how a Pivot skill goes to PivotPath planner). + * + * @param intent RobotIntent msg + * + * @return PlanRequest based on input RobotIntent + */ + PlanRequest make_request(const RobotIntent& intent); + + /* + * @brief Get a Trajectory based on the string name given in MotionCommand. + * Guaranteed to output a valid Trajectory: defaults to + * EscapeObstaclesPathPlanner if requested planner fails, and gives WARN + * logs. + * + * @details Uses unsafe_plan_for_robot() to get a plan, handling any + * Exceptions that come up. + * + * @param request PlanRequest to create a Trajectory from + * + * @return Trajectory (timestamped series of poses & twists) that satisfies + * the PlanRequest as well as possible + */ + Trajectory safe_plan_for_robot(const planning::PlanRequest& request); + + /* + * @brief Get a Trajectory based on the string name given in MotionCommand. + * + * @details Differs from safe_plan_for_robot() in that it will throw Exceptions if + * planners fail (which safe_plan_for_robot() handles). + * + * @param request PlanRequest to create a Trajectory from + * + * @return Trajectory (timestamped series of poses & twists) that satisfies + * the PlanRequest as well as possible + */ + Trajectory unsafe_plan_for_robot(const planning::PlanRequest& request); + + /* + * @brief Check that robot is visible in world_state and that world_state has been + * updated recently. + */ + [[nodiscard]] bool robot_alive() const; + + rclcpp::Node* node_; + + // unique_ptrs here because we don't want to transfer ownership of + // thePathPlanner objects anywhere else + // (must be some type of ptr for polymorphism) + std::unordered_map> path_planners_; + // EscapeObstaclesPathPlanner is our default path planner + // because when robots start inside of an obstacle, all other planners will fail + // TODO(Kevin): make EscapeObstaclesPathPlanner default start of all planner FSM so this doesn't + // happen + std::unique_ptr default_path_planner_{ + std::make_unique()}; + + // raw ptr here because current_path_planner_ should not take ownership + // from any of the unique_ptrs to PathPlanners + PathPlanner* current_path_planner_{default_path_planner_.get()}; + + int robot_id_; + TrajectoryCollection* robot_trajectories_; + const GlobalState& global_state_; + + bool had_break_beam_ = false; + + rclcpp::Subscription::SharedPtr intent_sub_; + rclcpp::Subscription::SharedPtr robot_status_sub_; + rclcpp::Publisher::SharedPtr trajectory_topic_; + rclcpp::Publisher::SharedPtr manipulator_pub_; + rclcpp::Service::SharedPtr hypothetical_path_service_; + + rj_drawing::RosDebugDrawer debug_draw_; +}; + +} // namespace planning diff --git a/soccer/src/soccer/planning/planner_node.cpp b/soccer/src/soccer/planning/planner_node.cpp index 55adb2f21ef..015f73bd6a5 100644 --- a/soccer/src/soccer/planning/planner_node.cpp +++ b/soccer/src/soccer/planning/planner_node.cpp @@ -7,14 +7,6 @@ #include #include "instant.hpp" -#include "planning/planner/collect_path_planner.hpp" -#include "planning/planner/escape_obstacles_path_planner.hpp" -#include "planning/planner/goalie_idle_path_planner.hpp" -#include "planning/planner/intercept_path_planner.hpp" -#include "planning/planner/line_kick_path_planner.hpp" -#include "planning/planner/path_target_path_planner.hpp" -#include "planning/planner/pivot_path_planner.hpp" -#include "planning/planner/settle_path_planner.hpp" namespace planning { @@ -144,257 +136,4 @@ void PlannerNode::execute(const std::shared_ptr goal_handle robot_task.is_executing = false; } -PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node, - TrajectoryCollection* robot_trajectories, - const GlobalState& global_state) - : node_{node}, - robot_id_{robot_id}, - robot_trajectories_{robot_trajectories}, - global_state_{global_state}, - debug_draw_{ - node->create_publisher(viz::topics::kDebugDrawTopic, 10), - fmt::format("planning_{}", robot_id)} { - // create map of {planner name -> planner} - path_planners_[GoalieIdlePathPlanner().name()] = std::make_unique(); - path_planners_[InterceptPathPlanner().name()] = std::make_unique(); - path_planners_[PathTargetPathPlanner().name()] = std::make_unique(); - path_planners_[SettlePathPlanner().name()] = std::make_unique(); - path_planners_[CollectPathPlanner().name()] = std::make_unique(); - path_planners_[LineKickPathPlanner().name()] = std::make_unique(); - path_planners_[PivotPathPlanner().name()] = std::make_unique(); - path_planners_[EscapeObstaclesPathPlanner().name()] = - std::make_unique(); - - // publish paths to control - trajectory_topic_ = node_->create_publisher( - planning::topics::trajectory_topic(robot_id), rclcpp::QoS(1).transient_local()); - - // publish kicker/dribbler cmds directly to radio - manipulator_pub_ = node->create_publisher( - control::topics::manipulator_setpoint_topic(robot_id), rclcpp::QoS(10)); - - // for ball sense and possession - robot_status_sub_ = node_->create_subscription( - radio::topics::robot_status_topic(robot_id), rclcpp::QoS(1), - [this](rj_msgs::msg::RobotStatus::SharedPtr status) { // NOLINT - had_break_beam_ = status->has_ball_sense; - }); - - // For hypothetical path planning - hypothetical_path_service_ = node_->create_service( - fmt::format("hypothetical_trajectory_robot_{}", robot_id), - [this](const std::shared_ptr request, - std::shared_ptr response) { - plan_hypothetical_robot_path(request, response); - }); -} - -void PlannerForRobot::execute_intent(const RobotIntent& intent) { - if (robot_alive()) { - // plan a path and send it to control - auto plan_request = make_request(intent); - - auto trajectory = safe_plan_for_robot(plan_request); - trajectory_topic_->publish(rj_convert::convert_to_ros(trajectory)); - - // send the kick/dribble commands to the radio - manipulator_pub_->publish(rj_msgs::build() - .shoot_mode(intent.shoot_mode) - .trigger_mode(intent.trigger_mode) - .kick_speed(intent.kick_speed) - .dribbler_speed(plan_request.dribbler_speed)); - - /* - // TODO (PR #1970): fix TrajectoryCollection - // store all latest trajectories in a mutex-locked shared map - robot_trajectories_->put(robot_id_, std::make_shared(std::move(trajectory)), - intent.priority); - */ - } -} - -void PlannerForRobot::plan_hypothetical_robot_path( - const std::shared_ptr& request, - std::shared_ptr& response) { - /* const auto intent = rj_convert::convert_from_ros(request->intent); */ - /* auto plan_request = make_request(intent); */ - /* auto trajectory = safe_plan_for_robot(plan_request); */ - /* RJ::Seconds trajectory_duration = trajectory.duration(); */ - /* response->estimate = rj_convert::convert_to_ros(trajectory_duration); */ -} - -std::optional PlannerForRobot::get_time_left() const { - // TODO(p-nayak): why does this say 3s even when the robot is on its point? - // get the Traj out of the relevant [Trajectory, priority] tuple in - // robot_trajectories_ - - /* - // TODO (PR #1970): fix TrajectoryCollection - const auto& [latest_traj, priority] = robot_trajectories_->get(robot_id_); - if (!latest_traj) { - return std::nullopt; - } - return latest_traj->end_time() - RJ::now(); - */ - return std::nullopt; -} - -PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { - // pass global_state_ directly - const auto* world_state = global_state_.world_state(); - const auto goalie_id = global_state_.goalie_id(); - const auto play_state = global_state_.play_state(); - const auto min_dist_from_ball = global_state_.coach_state().global_override.min_dist_from_ball; - const auto max_robot_speed = global_state_.coach_state().global_override.max_speed; - const auto max_dribbler_speed = global_state_.coach_state().global_override.max_dribbler_speed; - const auto& robot = world_state->our_robots.at(robot_id_); - const auto start = RobotInstant{robot.pose, robot.velocity, robot.timestamp}; - - const auto global_obstacles = global_state_.global_obstacles(); - rj_geometry::ShapeSet real_obstacles = global_obstacles; - - const auto def_area_obstacles = global_state_.def_area_obstacles(); - rj_geometry::ShapeSet virtual_obstacles = intent.local_obstacles; - const bool is_goalie = goalie_id == robot_id_; - if (!is_goalie) { - virtual_obstacles.add(def_area_obstacles); - } - - /* - // TODO (PR #1970): fix TrajectoryCollection - // make a copy instead of getting the actual shared_ptr to Trajectory - std::array, kNumShells> planned_trajectories; - - for (size_t i = 0; i < kNumShells; i++) { - // TODO(Kevin): check that priority works (seems like - // robot_trajectories_ is passed on init, when no planning has occured - // yet) - const auto& [trajectory, priority] = robot_trajectories_->get(i); - if (i != robot_id_ && priority >= intent.priority) { - if (!trajectory) { - planned_trajectories[i] = std::nullopt; - } else { - planned_trajectories[i] = std::make_optional(*trajectory.get()); - } - } - } - */ - - RobotConstraints constraints; - MotionCommand motion_command; - // Attempting to create trajectories with max speeds <= 0 crashes the planner (during RRT - // generation) - if (max_robot_speed == 0.0f) { - // If coach node has speed set to 0, - // force HALT by replacing the MotionCommand with an empty one. - motion_command = MotionCommand{}; - } else if (max_robot_speed < 0.0f) { - // If coach node has speed set to negative, assume infinity. - // Negative numbers cause crashes, but 10 m/s is an effectively infinite limit. - motion_command = intent.motion_command; - constraints.mot.max_speed = 10.0f; - } else { - motion_command = intent.motion_command; - constraints.mot.max_speed = max_robot_speed; - } - - float dribble_speed = - std::min(static_cast(intent.dribbler_speed), static_cast(max_dribbler_speed)); - - return PlanRequest{start, - motion_command, - constraints, - std::move(real_obstacles), - std::move(virtual_obstacles), - robot_trajectories_, - static_cast(robot_id_), - world_state, - intent.priority, - &debug_draw_, - had_break_beam_, - min_dist_from_ball, - dribble_speed}; -} - -Trajectory PlannerForRobot::unsafe_plan_for_robot(const planning::PlanRequest& request) { - if (path_planners_.count(request.motion_command.name) == 0) { - throw std::runtime_error(fmt::format("ID {}: MotionCommand name <{}> does not exist!", - robot_id_, request.motion_command.name)); - } - - // get Trajectory from the planner requested in MotionCommand - current_path_planner_ = path_planners_[request.motion_command.name].get(); - Trajectory trajectory = current_path_planner_->plan(request); - - if (trajectory.empty()) { - // empty Trajectory means current_path_planner_ has failed - // if current_path_planner_ fails, reset it before throwing exception - current_path_planner_->reset(); - throw std::runtime_error(fmt::format("PathPlanner <{}> failed to create valid Trajectory!", - current_path_planner_->name())); - } - - if (!trajectory.angles_valid()) { - throw std::runtime_error(fmt::format("Trajectory returned from <{}> has no angle profile!", - current_path_planner_->name())); - } - - if (!trajectory.time_created().has_value()) { - throw std::runtime_error(fmt::format("Trajectory returned from <{}> has no timestamp!", - current_path_planner_->name())); - } - - return trajectory; -} - -Trajectory PlannerForRobot::safe_plan_for_robot(const planning::PlanRequest& request) { - Trajectory trajectory; - try { - trajectory = unsafe_plan_for_robot(request); - } catch (std::runtime_error exception) { - SPDLOG_WARN("PlannerForRobot {} error caught: {}", robot_id_, exception.what()); - SPDLOG_WARN("PlannerForRobot {}: Defaulting to EscapeObstaclesPathPlanner", robot_id_); - - current_path_planner_ = default_path_planner_.get(); - trajectory = current_path_planner_->plan(request); - // TODO(Kevin): planning should be able to send empty Trajectory - // without crashing, instead of resorting to default planner - // (currently the ros_convert throws "cannot serialize trajectory with - // invalid angles") - } - - // draw robot's desired path - std::vector path; - std::transform(trajectory.instants().begin(), trajectory.instants().end(), - std::back_inserter(path), - [](const auto& instant) { return instant.position(); }); - debug_draw_.draw_path(path); - - // draw robot's desired endpoint - debug_draw_.draw_circle(rj_geometry::Circle(path.back(), kRobotRadius), Qt::black); - - // draw obstacles for this robot - // TODO: these will stack atop each other, since each robot draws obstacles - debug_draw_.draw_shapes(global_state_.global_obstacles(), QColor(255, 0, 0, 30)); - debug_draw_.draw_shapes(request.virtual_obstacles, QColor(255, 0, 0, 30)); - debug_draw_.publish(); - - return trajectory; -} - -bool PlannerForRobot::robot_alive() const { - return global_state_.world_state()->our_robots.at(robot_id_).visible && - RJ::now() < global_state_.world_state()->last_updated_time + RJ::Seconds(PARAM_timeout); -} - -bool PlannerForRobot::is_done() const { - // no segfaults - if (current_path_planner_ == nullptr) { - return false; - } - - return current_path_planner_->is_done(); -} - } // namespace planning diff --git a/soccer/src/soccer/planning/planner_node.hpp b/soccer/src/soccer/planning/planner_node.hpp index 4c64002c883..f52d2eb7111 100644 --- a/soccer/src/soccer/planning/planner_node.hpp +++ b/soccer/src/soccer/planning/planner_node.hpp @@ -9,249 +9,21 @@ #include #include -#include #include -#include -#include -#include -#include -#include #include #include "node.hpp" #include "planner/path_planner.hpp" #include "planner/plan_request.hpp" -#include "planning/planner/escape_obstacles_path_planner.hpp" #include "planning/trajectory_collection.hpp" #include "planning_params.hpp" #include "robot_intent.hpp" #include "trajectory.hpp" #include "world_state.hpp" +#include "planner_for_robot.hpp" namespace planning { -/** - * Aggregates info from other ROS nodes into an unchanging "global state" for - * each planner. Read-only (from PlannerNode's perspective). - * - * ("Global state" in quotes since many of these fields can be changed by other - * nodes; however, to PlannerNode these are immutable.) - */ -class GlobalState { -public: - GlobalState(rclcpp::Node* node) { - play_state_sub_ = node->create_subscription( - referee::topics::kPlayStateTopic, rclcpp::QoS(1), - [this](rj_msgs::msg::PlayState::SharedPtr state) { // NOLINT - last_play_state_ = rj_convert::convert_from_ros(*state); - }); - game_settings_sub_ = node->create_subscription( - config_server::topics::kGameSettingsTopic, rclcpp::QoS(1), - [this](rj_msgs::msg::GameSettings::SharedPtr settings) { // NOLINT - last_game_settings_ = rj_convert::convert_from_ros(*settings); - }); - goalie_sub_ = node->create_subscription( - referee::topics::kGoalieTopic, rclcpp::QoS(1), - [this](rj_msgs::msg::Goalie::SharedPtr goalie) { // NOLINT - last_goalie_id_ = goalie->goalie_id; - }); - global_obstacles_sub_ = node->create_subscription( - planning::topics::kGlobalObstaclesTopic, rclcpp::QoS(1), - [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr global_obstacles) { // NOLINT - last_global_obstacles_ = rj_convert::convert_from_ros(*global_obstacles); - }); - def_area_obstacles_sub_ = node->create_subscription( - planning::topics::kDefAreaObstaclesTopic, rclcpp::QoS(1), - [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr def_area_obstacles) { // NOLINT - last_def_area_obstacles_ = rj_convert::convert_from_ros(*def_area_obstacles); - }); - world_state_sub_ = node->create_subscription( - vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), - [this](rj_msgs::msg::WorldState::SharedPtr world_state) { // NOLINT - last_world_state_ = rj_convert::convert_from_ros(*world_state); - }); - coach_state_sub_ = node->create_subscription( - "/strategy/coach_state", rclcpp::QoS(1), - [this](rj_msgs::msg::CoachState::SharedPtr coach_state) { // NOLINT - last_coach_state_ = *coach_state; - }); - } - - [[nodiscard]] PlayState play_state() const { - return last_play_state_; - } - [[nodiscard]] GameSettings game_settings() const { - return last_game_settings_; - } - [[nodiscard]] int goalie_id() const { - return last_goalie_id_; - } - [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const { - return last_global_obstacles_; - } - [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const { - return last_def_area_obstacles_; - } - [[nodiscard]] const WorldState* world_state() const { - return &last_world_state_; - } - [[nodiscard]] const rj_msgs::msg::CoachState coach_state() const { - return last_coach_state_; - } - -private: - rclcpp::Subscription::SharedPtr play_state_sub_; - rclcpp::Subscription::SharedPtr game_settings_sub_; - rclcpp::Subscription::SharedPtr goalie_sub_; - rclcpp::Subscription::SharedPtr global_obstacles_sub_; - rclcpp::Subscription::SharedPtr def_area_obstacles_sub_; - rclcpp::Subscription::SharedPtr world_state_sub_; - rclcpp::Subscription::SharedPtr coach_state_sub_; - - PlayState last_play_state_ = PlayState::halt(); - GameSettings last_game_settings_; - int last_goalie_id_; - rj_geometry::ShapeSet last_global_obstacles_; - rj_geometry::ShapeSet last_def_area_obstacles_; - WorldState last_world_state_; - rj_msgs::msg::CoachState last_coach_state_; -}; - -/** - * @brief Handles one robot's planning, which for us is a translation of RobotIntent to - * Trajectory. Bundles together all relevant ROS pub/subs and forwards - * RobotIntents to the appropriate PathPlanner (see path_planner.hpp). - * - * In general, prefers default behavior and WARN-level logs over crashing. - * - * (PlannerNode makes N PlannerForRobots and handles them all, see - * PlannerNode below.) - */ -class PlannerForRobot { -public: - PlannerForRobot(int robot_id, rclcpp::Node* node, TrajectoryCollection* robot_trajectories, - const GlobalState& global_state); - - PlannerForRobot(PlannerForRobot&&) = delete; - const PlannerForRobot& operator=(PlannerForRobot&&) = delete; - PlannerForRobot(const PlannerForRobot&) = delete; - const PlannerForRobot& operator=(const PlannerForRobot&) = delete; - - ~PlannerForRobot() = default; - - /** - * Entry point for the planner node's ActionServer. - * - * Creates and publishes a Trajectory based on the given RobotIntent. Also - * publishes a ManipulatorSetpoint to control kicker/dribbler/chipper. - */ - void execute_intent(const RobotIntent& intent); - - /* - * @brief estimate the amount of time it would take for a robot to execute a robot intent - * (SERVICE). - * - * @param request Requested RobotIntent resulting in the hypothetical robot path. - * @param response The response object that will contain the resultant time to completion of a - * hypothetical path. - */ - // TODO(Kevin): I broke this, sorry - void plan_hypothetical_robot_path( - const std::shared_ptr& request, - std::shared_ptr& response); - - /** - * @return RJ::Seconds time left for the trajectory to complete - * - * Note: RJ::Seconds is an alias for std::chrono::duration. - */ - [[nodiscard]] std::optional get_time_left() const; - - /* - * @return true if current planner is done, false otherwise. - */ - [[nodiscard]] bool is_done() const; - -private: - /** - * @brief Create a PlanRequest based on the given RobotIntent. - * - * @details This is how gameplay's RobotIntents end up in the right planner (e.g. - * how a Pivot skill goes to PivotPath planner). - * - * @param intent RobotIntent msg - * - * @return PlanRequest based on input RobotIntent - */ - PlanRequest make_request(const RobotIntent& intent); - - /* - * @brief Get a Trajectory based on the string name given in MotionCommand. - * Guaranteed to output a valid Trajectory: defaults to - * EscapeObstaclesPathPlanner if requested planner fails, and gives WARN - * logs. - * - * @details Uses unsafe_plan_for_robot() to get a plan, handling any - * Exceptions that come up. - * - * @param request PlanRequest to create a Trajectory from - * - * @return Trajectory (timestamped series of poses & twists) that satisfies - * the PlanRequest as well as possible - */ - Trajectory safe_plan_for_robot(const planning::PlanRequest& request); - - /* - * @brief Get a Trajectory based on the string name given in MotionCommand. - * - * @details Differs from safe_plan_for_robot() in that it will throw Exceptions if - * planners fail (which safe_plan_for_robot() handles). - * - * @param request PlanRequest to create a Trajectory from - * - * @return Trajectory (timestamped series of poses & twists) that satisfies - * the PlanRequest as well as possible - */ - Trajectory unsafe_plan_for_robot(const planning::PlanRequest& request); - - /* - * @brief Check that robot is visible in world_state and that world_state has been - * updated recently. - */ - [[nodiscard]] bool robot_alive() const; - - rclcpp::Node* node_; - - // unique_ptrs here because we don't want to transfer ownership of - // thePathPlanner objects anywhere else - // (must be some type of ptr for polymorphism) - std::unordered_map> path_planners_; - // EscapeObstaclesPathPlanner is our default path planner - // because when robots start inside of an obstacle, all other planners will fail - // TODO(Kevin): make EscapeObstaclesPathPlanner default start of all planner FSM so this doesn't - // happen - std::unique_ptr default_path_planner_{ - std::make_unique()}; - - // raw ptr here because current_path_planner_ should not take ownership - // from any of the unique_ptrs to PathPlanners - PathPlanner* current_path_planner_{default_path_planner_.get()}; - - int robot_id_; - TrajectoryCollection* robot_trajectories_; - const GlobalState& global_state_; - - bool had_break_beam_ = false; - - rclcpp::Subscription::SharedPtr intent_sub_; - rclcpp::Subscription::SharedPtr robot_status_sub_; - rclcpp::Publisher::SharedPtr trajectory_topic_; - rclcpp::Publisher::SharedPtr manipulator_pub_; - rclcpp::Service::SharedPtr hypothetical_path_service_; - - rj_drawing::RosDebugDrawer debug_draw_; -}; - /** * ROS node that spawns many PlannerForRobots and helps coordinate them. */ From 15319a6012180aa3c7c7cd85772067dab4b094a6 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 5 Nov 2023 21:53:16 -0500 Subject: [PATCH 2/8] Fix Code Style On split-up-planner (#2140) automated style fixes Co-authored-by: petergarud --- soccer/src/soccer/planning/global_state.hpp | 25 ++++++------------- .../src/soccer/planning/planner_for_robot.hpp | 1 + soccer/src/soccer/planning/planner_node.hpp | 2 +- 3 files changed, 9 insertions(+), 19 deletions(-) diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp index 906e1234485..d28f41b8a31 100644 --- a/soccer/src/soccer/planning/global_state.hpp +++ b/soccer/src/soccer/planning/global_state.hpp @@ -3,6 +3,7 @@ #include #include #include + #include "planning/planner/escape_obstacles_path_planner.hpp" namespace planning { @@ -54,27 +55,15 @@ class GlobalState { }); } - [[nodiscard]] PlayState play_state() const { - return last_play_state_; - } - [[nodiscard]] GameSettings game_settings() const { - return last_game_settings_; - } - [[nodiscard]] int goalie_id() const { - return last_goalie_id_; - } - [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const { - return last_global_obstacles_; - } + [[nodiscard]] PlayState play_state() const { return last_play_state_; } + [[nodiscard]] GameSettings game_settings() const { return last_game_settings_; } + [[nodiscard]] int goalie_id() const { return last_goalie_id_; } + [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const { return last_global_obstacles_; } [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const { return last_def_area_obstacles_; } - [[nodiscard]] const WorldState* world_state() const { - return &last_world_state_; - } - [[nodiscard]] const rj_msgs::msg::CoachState coach_state() const { - return last_coach_state_; - } + [[nodiscard]] const WorldState* world_state() const { return &last_world_state_; } + [[nodiscard]] const rj_msgs::msg::CoachState coach_state() const { return last_coach_state_; } private: rclcpp::Subscription::SharedPtr play_state_sub_; diff --git a/soccer/src/soccer/planning/planner_for_robot.hpp b/soccer/src/soccer/planning/planner_for_robot.hpp index ee45e2ff1e5..bccc8d95cd7 100644 --- a/soccer/src/soccer/planning/planner_for_robot.hpp +++ b/soccer/src/soccer/planning/planner_for_robot.hpp @@ -3,6 +3,7 @@ #include #include #include + #include "global_state.hpp" namespace planning { diff --git a/soccer/src/soccer/planning/planner_node.hpp b/soccer/src/soccer/planning/planner_node.hpp index f52d2eb7111..3f18ae326be 100644 --- a/soccer/src/soccer/planning/planner_node.hpp +++ b/soccer/src/soccer/planning/planner_node.hpp @@ -15,12 +15,12 @@ #include "node.hpp" #include "planner/path_planner.hpp" #include "planner/plan_request.hpp" +#include "planner_for_robot.hpp" #include "planning/trajectory_collection.hpp" #include "planning_params.hpp" #include "robot_intent.hpp" #include "trajectory.hpp" #include "world_state.hpp" -#include "planner_for_robot.hpp" namespace planning { From 06373122623669b35d230f37d96a8994c43268d0 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 6 Nov 2023 19:23:56 +0000 Subject: [PATCH 3/8] updated includes --- soccer/src/soccer/planning/global_state.hpp | 2 -- soccer/src/soccer/planning/planner_for_robot.hpp | 1 + 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp index d28f41b8a31..499f6762b1c 100644 --- a/soccer/src/soccer/planning/global_state.hpp +++ b/soccer/src/soccer/planning/global_state.hpp @@ -4,8 +4,6 @@ #include #include -#include "planning/planner/escape_obstacles_path_planner.hpp" - namespace planning { /** diff --git a/soccer/src/soccer/planning/planner_for_robot.hpp b/soccer/src/soccer/planning/planner_for_robot.hpp index bccc8d95cd7..b83bebf2d91 100644 --- a/soccer/src/soccer/planning/planner_for_robot.hpp +++ b/soccer/src/soccer/planning/planner_for_robot.hpp @@ -3,6 +3,7 @@ #include #include #include +#include "planning/planner/escape_obstacles_path_planner.hpp" #include "global_state.hpp" From 1da96ab837cb7405f4f2ee23f98ed1051029869a Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 6 Nov 2023 14:27:27 -0500 Subject: [PATCH 4/8] Fix Code Style On split-up-planner (#2141) automated style fixes Co-authored-by: petergarud --- soccer/src/soccer/planning/planner_for_robot.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/planning/planner_for_robot.hpp b/soccer/src/soccer/planning/planner_for_robot.hpp index b83bebf2d91..7b780f5be83 100644 --- a/soccer/src/soccer/planning/planner_for_robot.hpp +++ b/soccer/src/soccer/planning/planner_for_robot.hpp @@ -3,9 +3,9 @@ #include #include #include -#include "planning/planner/escape_obstacles_path_planner.hpp" #include "global_state.hpp" +#include "planning/planner/escape_obstacles_path_planner.hpp" namespace planning { From e9dedac9576f2694f867d9c93a86d96f32a199cf Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 6 Nov 2023 20:11:37 +0000 Subject: [PATCH 5/8] updated includes in global_state --- soccer/src/soccer/planning/global_state.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp index 499f6762b1c..5412dda366d 100644 --- a/soccer/src/soccer/planning/global_state.hpp +++ b/soccer/src/soccer/planning/global_state.hpp @@ -1,6 +1,8 @@ #pragma once +#include #include +#include #include #include From 5d2df5cf714e2aba8d227227ff6036eec764fbce Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 6 Nov 2023 15:13:50 -0500 Subject: [PATCH 6/8] Fix Code Style On split-up-planner (#2142) automated style fixes Co-authored-by: petergarud --- soccer/src/soccer/planning/global_state.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp index 5412dda366d..ca281ad0b87 100644 --- a/soccer/src/soccer/planning/global_state.hpp +++ b/soccer/src/soccer/planning/global_state.hpp @@ -1,6 +1,7 @@ #pragma once #include + #include #include #include From fe2bfce3456050d823b6487ca4ded84ec32cb0cd Mon Sep 17 00:00:00 2001 From: petergarud Date: Tue, 7 Nov 2023 00:32:11 +0000 Subject: [PATCH 7/8] removed unnecessary includes from planner_for_robot.hpp and global_state.hpp --- soccer/src/soccer/planning/global_state.hpp | 7 ++++++- soccer/src/soccer/planning/planner_for_robot.hpp | 11 +++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp index ca281ad0b87..12dd27924db 100644 --- a/soccer/src/soccer/planning/global_state.hpp +++ b/soccer/src/soccer/planning/global_state.hpp @@ -1,11 +1,16 @@ #pragma once #include +#include #include -#include +#include #include #include +#include +#include "world_state.hpp" +#include "game_state.hpp" +#include "game_settings.hpp" namespace planning { diff --git a/soccer/src/soccer/planning/planner_for_robot.hpp b/soccer/src/soccer/planning/planner_for_robot.hpp index 7b780f5be83..c0990d1a9c2 100644 --- a/soccer/src/soccer/planning/planner_for_robot.hpp +++ b/soccer/src/soccer/planning/planner_for_robot.hpp @@ -1,10 +1,21 @@ #pragma once +#include +#include +#include + +#include #include #include #include +#include #include "global_state.hpp" +#include "robot_intent.hpp" +#include "planning/trajectory.hpp" +#include "planning/trajectory_collection.hpp" +#include "planning/planner/plan_request.hpp" +#include "planning/planner/path_planner.hpp" #include "planning/planner/escape_obstacles_path_planner.hpp" namespace planning { From c611b7ab59c59baf30d0bfd4eba11eda9ce60d38 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 6 Nov 2023 19:35:08 -0500 Subject: [PATCH 8/8] Fix Code Style On split-up-planner (#2143) automated style fixes Co-authored-by: petergarud --- soccer/src/soccer/planning/global_state.hpp | 9 +++++---- soccer/src/soccer/planning/planner_for_robot.hpp | 13 +++++++------ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp index 12dd27924db..fc62e1de750 100644 --- a/soccer/src/soccer/planning/global_state.hpp +++ b/soccer/src/soccer/planning/global_state.hpp @@ -1,16 +1,17 @@ #pragma once #include -#include #include +#include #include #include -#include #include -#include "world_state.hpp" -#include "game_state.hpp" +#include + #include "game_settings.hpp" +#include "game_state.hpp" +#include "world_state.hpp" namespace planning { diff --git a/soccer/src/soccer/planning/planner_for_robot.hpp b/soccer/src/soccer/planning/planner_for_robot.hpp index c0990d1a9c2..22ef9ecf447 100644 --- a/soccer/src/soccer/planning/planner_for_robot.hpp +++ b/soccer/src/soccer/planning/planner_for_robot.hpp @@ -1,22 +1,23 @@ #pragma once -#include #include +#include #include #include + +#include #include #include #include -#include #include "global_state.hpp" -#include "robot_intent.hpp" +#include "planning/planner/escape_obstacles_path_planner.hpp" +#include "planning/planner/path_planner.hpp" +#include "planning/planner/plan_request.hpp" #include "planning/trajectory.hpp" #include "planning/trajectory_collection.hpp" -#include "planning/planner/plan_request.hpp" -#include "planning/planner/path_planner.hpp" -#include "planning/planner/escape_obstacles_path_planner.hpp" +#include "robot_intent.hpp" namespace planning {