From c32025a33c1a7e492ca8cc7c4ef9d62d2f27b9da Mon Sep 17 00:00:00 2001 From: anson Date: Sun, 10 Mar 2024 21:20:00 -0400 Subject: [PATCH] added goal obstacles --- rj_common/include/rj_common/field_dimensions.hpp | 4 ++-- soccer/src/soccer/planning/global_state.cpp | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/rj_common/include/rj_common/field_dimensions.hpp b/rj_common/include/rj_common/field_dimensions.hpp index d97a5e1cf76..a477fbf9de2 100644 --- a/rj_common/include/rj_common/field_dimensions.hpp +++ b/rj_common/include/rj_common/field_dimensions.hpp @@ -233,8 +233,8 @@ struct FieldDimensions { their_left_goal_post_coordinate_ = rj_geometry::Point(-goal_width_ / 2, length_); their_right_goal_post_coordinate_ = rj_geometry::Point(goal_width_ / 2, length_); - our_goal_area_ = rj_geometry::Rect(our_left_goal_post_coordinate_, our_right_goal_post_coordinate_ + goal_depth_); - their_goal_area_ = rj_geometry::Rect(their_left_goal_post_coordinate_, their_right_goal_post_coordinate_ - goal_depth_); + our_goal_area_ = rj_geometry::Rect(our_left_goal_post_coordinate_, our_right_goal_post_coordinate_ - rj_geometry::Point(0, goal_depth_)); + their_goal_area_ = rj_geometry::Rect(their_left_goal_post_coordinate_, their_right_goal_post_coordinate_ + rj_geometry::Point(0, goal_depth_)); our_left_corner_ = rj_geometry::Point(field_x_left_coord_, 0.0); our_right_corner_ = rj_geometry::Point(field_x_right_coord_, 0.0); diff --git a/soccer/src/soccer/planning/global_state.cpp b/soccer/src/soccer/planning/global_state.cpp index d8efd92b54c..c630327f23a 100644 --- a/soccer/src/soccer/planning/global_state.cpp +++ b/soccer/src/soccer/planning/global_state.cpp @@ -85,6 +85,9 @@ rj_geometry::ShapeSet GlobalState::create_defense_area_obstacles() { auto our_goal_area{ std::make_shared(last_field_dimensions_.our_goal_area())}; + auto their_goal_area{ + std::make_shared(last_field_dimensions_.their_goal_area())}; + // auto their_goal_area{ // std::make_shared(last_field_dimensions_.our_goal_area())}; @@ -107,6 +110,8 @@ rj_geometry::ShapeSet GlobalState::create_defense_area_obstacles() { def_area_obstacles.add(our_defense_area); def_area_obstacles.add(our_goal_area); def_area_obstacles.add(their_defense_area); + def_area_obstacles.add(their_goal_area); + return def_area_obstacles; }