diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3290c69877..9392d80fe5 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -94,7 +94,7 @@ jobs: id: style-check run: | # check formatting style (C++) - git diff -U0 --no-color ${{ github.base_ref }} ${{ github.head_ref }} | python3 util/clang-format-diff.py -binary clang-format-12 -i -p1 + git diff -U0 --no-color ${{ github.base_ref }} ${{ github.head_ref }} | python3 util/clang-format-diff.py -binary clang-format-14 -i -p1 if ! git diff-index --quiet HEAD; then echo "::set-output name=changed::true" diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp index cf8836c01f..7f91db2388 100644 --- a/soccer/src/soccer/strategy/agent/position/defense.cpp +++ b/soccer/src/soccer/strategy/agent/position/defense.cpp @@ -142,8 +142,8 @@ std::optional Defense::state_to_task(RobotIntent intent) { intent.is_active = true; return intent; } else if (current_state_ == WALLING) { - if (!walling_robots_.empty()) { - Waller waller{waller_id_, (int)walling_robots_.size()}; + if (!walling_robots_.empty() && waller_id_ != -1) { + Waller waller{waller_id_, walling_robots_}; return waller.get_task(intent, last_world_state_, this->field_dimensions_); } } else if (current_state_ == FACING) { diff --git a/soccer/src/soccer/strategy/agent/position/waller.cpp b/soccer/src/soccer/strategy/agent/position/waller.cpp index 6919cdb7eb..c841f7dc4d 100644 --- a/soccer/src/soccer/strategy/agent/position/waller.cpp +++ b/soccer/src/soccer/strategy/agent/position/waller.cpp @@ -2,19 +2,14 @@ namespace strategy { -Waller::Waller(int waller_num, int total_wallers) { +Waller::Waller(int waller_num, std::vector walling_robots) { defense_type_ = "Waller"; waller_pos_ = waller_num; - total_wallers_ = total_wallers; + walling_robots_ = walling_robots; } std::optional Waller::get_task(RobotIntent intent, const WorldState* world_state, FieldDimensions field_dimensions) { - rj_geometry::Point ball_location{world_state->ball.position}; - - // Get Goal Location (Always (0,0)) as of creation - rj_geometry::Point goal_center_point{0, 0}; - // Creates Minimum wall radius is slightly greater than box bounds // Dimension accessors should be edited when we figure out how we are doing dimensions realtime // from vision @@ -24,39 +19,62 @@ std::optional Waller::get_task(RobotIntent intent, const WorldState double min_wall_rad{(kRobotRadius * 4.0f) + line_w + hypot(static_cast(box_w) / 2, static_cast((box_h)))}; - // Find ball_direction unit vector - rj_geometry::Point ball_dir_vector{(ball_location - goal_center_point)}; - // (avoid div by 0) - ball_dir_vector /= (ball_dir_vector.mag() + 0.000001); + auto ball_pos = world_state->ball.position; - // Find target Point - rj_geometry::Point mid_point{(goal_center_point) + (ball_dir_vector * min_wall_rad)}; + auto robot_id = intent.robot_id; - // Calculate the wall spacing - auto wall_spacing = robot_diameter_multiplier_ * kRobotDiameter + kBallRadius; + auto robot_pos = world_state->get_robot(true, robot_id).pose.position(); + auto goal_pos = rj_geometry::Point{0, 0}; + auto num_wallers = walling_robots_.size(); - // Calculate the target point - rj_geometry::Point target_point{}; + // Find ball_direction unit vector + rj_geometry::Point ball_dir_vector{(ball_pos - goal_pos)}; - target_point = - mid_point + - ball_dir_vector * ((total_wallers_ - waller_pos_ - (total_wallers_ / 2)) * wall_spacing); + ball_dir_vector = ball_dir_vector.normalized(); - target_point = target_point.rotate(mid_point, M_PI / 2); + // Find target Point + rj_geometry::Point mid_point{(goal_pos) + (ball_dir_vector * min_wall_rad)}; - // Stop at end of path - rj_geometry::Point target_vel{0.0, 0.0}; + auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius; - // Face ball - planning::PathTargetFaceOption face_option{planning::FaceBall{}}; + rj_geometry::Point target_point{}; + auto angle = (mid_point - goal_pos).angle(); + auto delta_angle = (wall_spacing * (waller_pos_ - num_wallers / 2. - 0.5)) / min_wall_rad; + auto target_angle = angle - delta_angle; - // Avoid ball - bool ignore_ball{true}; + target_point = + (goal_pos + rj_geometry::Point{1, 0}).normalized(min_wall_rad).rotated(target_angle); + + if (abs(robot_pos.dist_to(goal_pos) - min_wall_rad) < kRobotRadius && + robot_pos.dist_to(target_point) > kRobotRadius) { + uint8_t parent_id; + + if (target_point.x() < robot_pos.x()) { + parent_id = walling_robots_[waller_pos_ - 2]; + } else { + parent_id = walling_robots_[waller_pos_]; + } + + if ((target_point.x() < robot_pos.x() && waller_pos_ != 1) || + (target_point.x() > robot_pos.x() && waller_pos_ != num_wallers)) { + auto parent_point = world_state->get_robot(true, parent_id).pose.position(); + angle = (parent_point - goal_pos).angle(); + delta_angle = wall_spacing / min_wall_rad; + target_angle = + angle + delta_angle * (signbit(target_point.x() - robot_pos.x()) ? -1 : 1); + + target_point = (goal_pos + rj_geometry::Point{1, 0}) + .normalized(min_wall_rad) + .rotated(target_angle); + } + } + + auto location_instant = planning::LinearMotionInstant{target_point, rj_geometry::Point{}}; + + auto target_cmd = + planning::MotionCommand{"path_target", location_instant, planning::FaceBall{}}; + intent.motion_command = target_cmd; - // Create Motion Command - planning::LinearMotionInstant target{target_point, target_vel}; - intent.motion_command = - planning::MotionCommand{"path_target", target, face_option, ignore_ball}; return intent; } diff --git a/soccer/src/soccer/strategy/agent/position/waller.hpp b/soccer/src/soccer/strategy/agent/position/waller.hpp index ab06c8b7d5..41a3bf3a6b 100644 --- a/soccer/src/soccer/strategy/agent/position/waller.hpp +++ b/soccer/src/soccer/strategy/agent/position/waller.hpp @@ -26,7 +26,7 @@ namespace strategy { */ class Waller : public RoleInterface { public: - Waller(int waller_num, int total_wallers); + Waller(int waller_num, std::vector walling_robots); ~Waller() = default; /** @@ -42,9 +42,8 @@ class Waller : public RoleInterface { private: std::string defense_type_; int waller_pos_; - int total_wallers_; - - static constexpr double robot_diameter_multiplier_ = 1.5; + static constexpr double kRobotDiameterMultiplier = 1.5; + std::vector walling_robots_; }; } // namespace strategy diff --git a/util/clang-format-diff.py b/util/clang-format-diff.py index 722796faf3..1c6e48457c 100644 --- a/util/clang-format-diff.py +++ b/util/clang-format-diff.py @@ -91,7 +91,7 @@ def main(): ) parser.add_argument( "-binary", - default="clang-format-12", + default="clang-format-14", help="location of binary to use for clang-format", ) args = parser.parse_args()