Ball placement obstacle with Stadium Shape on their ball placement

rj_common/include/rj_common/field_dimensions.hpp

@@ -7,6 +7,7 @@
 #include <rj_geometry/arc.hpp>
 #include <rj_geometry/circle.hpp>
 #include <rj_geometry/composite_shape.hpp>
+#include <rj_geometry/stadium_shape.hpp>
 #include <rj_geometry/geometry_conversions.hpp>
 #include <rj_geometry/line.hpp>
 #include <rj_geometry/point.hpp>

rj_geometry/include/rj_geometry/stadium_shape.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "point.hpp"
+#include "shape.hpp"
+#include "segment.hpp"
+#include "polygon.hpp"
+#include "shape_set.hpp"
+#include <vector>
+#include <memory>
+#include <set>
+
+namespace rj_geometry {
+
+/**
+ * A rj_geometry::StadiumShape is a Shape that is made up of 2 circles and a polygon. It represents the shape of a track from track and field.
+ */
+class StadiumShape : public Shape {
+public:
+    ~StadiumShape() = default;
+
+    StadiumShape() = default;
+
+    StadiumShape(Point c1, Point c2, float r) {
+        init(c1, c2, r);
+    }
+
+    [[nodiscard]] Shape* clone() const override;
+
+    [[nodiscard]] bool contains_point(Point pt) const override;
+    [[nodiscard]] bool near_point(Point pt, float threshold) const override;
+
+    [[nodiscard]] const std::vector<std::shared_ptr<Shape>>& subshapes() const {
+        return subshapes_;
+    }
+
+    [[nodiscard]] const rj_geometry::ShapeSet drawshapes() const {
+        return drawshapes_;
+    }
+
+protected:
+    void init(Point c1, Point c2, float r);
+
+private:
+    std::vector<std::shared_ptr<Shape>> subshapes_;
+    rj_geometry::ShapeSet drawshapes_;
+};
+
+}

rj_geometry/src/CMakeLists.txt

@@ -5,6 +5,7 @@ set(GEOMETRY2D_SRCS
     arc.cpp
     circle.cpp
     composite_shape.cpp
+    stadium_shape.cpp
     line.cpp
     point.cpp
     polygon.cpp

rj_geometry/src/stadium_shape.cpp

@@ -0,0 +1,50 @@
+#include <rj_geometry/stadium_shape.hpp>
+
+namespace rj_geometry {
+
+Shape* StadiumShape::clone() const { return new StadiumShape(*this); }
+
+void StadiumShape::init(Point c1, Point c2, float r) {
+    rj_geometry::Circle first_circle = rj_geometry::Circle{c1, static_cast<float>(r)};
+    rj_geometry::Circle second_circle = rj_geometry::Circle{c2, static_cast<float>(r)};
+
+    rj_geometry::Segment vect{c1, c2};
+
+    rj_geometry::Point end1{c1.x() + r * (c2.x() - c1.x()) / vect.length(), c1.y() + r * (c2.y() - c1.y()) / vect.length()};
+    rj_geometry::Point end2{c2.x() - r * (c2.x() - c1.x()) / vect.length(), c2.y() - r * (c2.y() - c1.y()) / vect.length()};
+
+    rj_geometry::Segment vect_updated{end1, end2};
+    rj_geometry::Polygon rect_obs{vect_updated, r};
+
+    std::shared_ptr<rj_geometry::Circle> c1_obs_ptr = std::make_shared<rj_geometry::Circle>(first_circle);
+    std::shared_ptr<rj_geometry::Polygon> rect_obs_ptr = std::make_shared<rj_geometry::Polygon>(rect_obs);
+    std::shared_ptr<rj_geometry::Circle> c2_obs_ptr = std::make_shared<rj_geometry::Circle>(second_circle);
+
+    subshapes_.push_back(c1_obs_ptr);
+    subshapes_.push_back(rect_obs_ptr);
+    subshapes_.push_back(c2_obs_ptr);
+
+    drawshapes_.add(c1_obs_ptr);
+    drawshapes_.add(rect_obs_ptr);
+    drawshapes_.add(c2_obs_ptr);
+}
+
+bool StadiumShape::contains_point(Point pt) const {
+    for (const auto& subshape : subshapes_) {
+        if (subshape->contains_point(pt)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+bool StadiumShape::near_point(Point pt, float threshold) const {
+    for (const auto& subshape : subshapes_) {
+        if (subshape->near_point(pt, threshold)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+}  // namespace rj_geometry

soccer/src/soccer/debug_drawer.cpp

@@ -48,6 +48,12 @@ void DebugDrawer::draw_circle(rj_geometry::Point center, float radius, const QCo
     dbg->set_color(color(qc));
 }
 
+// void DebugDrawer::draw_stadium(rj_geometry::StadiumShape& stadium, const QColor& qc, const QString& layer) {
+//     this->draw_circle(stadium.drawshapes()[0], qc, layer);
+//     this->draw_polygon(stadium.drawshapes()[1], qc, layer);
+//     this->draw_circle(stadium.drawshapes()[2], qc, layer);
+// }
+
 void DebugDrawer::draw_arc(const rj_geometry::Arc& arc, const QColor& qc, const QString& layer) {
     Packet::DebugArc* dbg = log_frame_.add_debug_arcs();
     dbg->set_layer(find_debug_layer(layer));

soccer/src/soccer/debug_drawer.hpp

@@ -4,6 +4,7 @@
 
 #include <rj_geometry/arc.hpp>
 #include <rj_geometry/composite_shape.hpp>
+#include <rj_geometry/stadium_shape.hpp>
 #include <rj_geometry/point.hpp>
 #include <rj_geometry/polygon.hpp>
 #include <rj_geometry/segment.hpp>
@@ -78,6 +79,10 @@ class DebugDrawer {
                      const QColor& qw = Qt::black,
                      const QString& layer = QString());
 
+    // void draw_stadium(rj_geometry::StadiumShape& stadium, 
+    //                 const QColor& qc = Qt::black, 
+    //                 const QString& layer = QString());
+
     /**
      * Fill the given log frame with the current debug drawing information,
      * and reset our current debug drawing data for the next cycle.

soccer/src/soccer/planning/planner/plan_request.cpp

@@ -78,6 +78,22 @@
         }
 
         out_static->add(std::make_shared<rj_geometry::Circle>(std::move(ball_obs)));
+
+        auto maybe_bp_point = in.play_state.ball_placement_point();
+        if (maybe_bp_point.has_value() && in.play_state.is_their_restart()) {
+            rj_geometry::Point bp_point = maybe_bp_point.value();
+            rj_geometry::StadiumShape stadium = rj_geometry::StadiumShape{in.world_state->ball.position, bp_point, ball_obs.radius()};
+
+            // for some reason adding the shared pointer below to our static obstacles breaks it, so we add the shape set it has instead.
+            // std::shared_ptr<rj_geometry::Shape> track_obs_ptr = std::make_shared<rj_geometry::Shape>(stadium);
+
+            out_static->add(stadium.drawshapes());
+
+            if (in.debug_drawer != nullptr) {
+                QColor draw_color = Qt::red;
+                in.debug_drawer->draw_stadium(stadium, draw_color);
+            }
+        }
     }
 }
 

soccer/src/soccer/ros_debug_drawer.hpp

@@ -58,6 +58,10 @@
                 color_from_qt(color)));
     }
 
+    void draw_stadium(const rj_geometry::StadiumShape& stadium, const QColor& color = QColor::fromRgb(0, 0, 0, 0)) {
+        this->draw_shapes(stadium.drawshapes(), color);
+    }
+
     void draw_segment(const rj_geometry::Segment& segment,
                       const QColor& color = QColor::fromRgb(0, 0, 0)) {
         frame_.segments.push_back(rj_drawing_msgs::build<rj_drawing_msgs::msg::DrawSegment>()