Ball placement obstacle with Stadium Shape on their ball placement (#…

…2294)

rj_common/include/rj_common/field_dimensions.hpp

@@ -13,6 +13,7 @@
 #include <rj_geometry/polygon.hpp>
 #include <rj_geometry/rect.hpp>
 #include <rj_geometry/shape_set.hpp>
+#include <rj_geometry/stadium_shape.hpp>
 #include <rj_msgs/msg/field_dimensions.hpp>
 
 using namespace std;

rj_geometry/include/rj_geometry/stadium_shape.hpp

@@ -0,0 +1,93 @@
+#pragma once
+
+#include <memory>
+#include <set>
+#include <vector>
+
+#include "point.hpp"
+#include "polygon.hpp"
+#include "segment.hpp"
+#include "shape.hpp"
+#include "shape_set.hpp"
+
+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); }
+
+    StadiumShape(const StadiumShape& other) {
+        for (const auto& shape : other.subshapes_) {
+            std::shared_ptr<Shape> itr_shape = std::shared_ptr<Shape>(shape->clone());
+            subshapes_.push_back(itr_shape);
+        }
+        drawshapes_ = other.drawshapes_;
+    }
+
+    [[nodiscard]] Shape* clone() const override;
+
+    [[nodiscard]] bool contains_point(Point pt) const override;
+    [[nodiscard]] bool near_point(Point pt, float threshold) const override;
+
+    using const_iterator = std::vector<std::shared_ptr<Shape>>::const_iterator;
+    using iterator = std::vector<std::shared_ptr<Shape>>::iterator;
+
+    [[nodiscard]] const_iterator begin() const { return subshapes_.begin(); }
+    [[nodiscard]] const_iterator end() const { return subshapes_.end(); }
+
+    iterator begin() { return subshapes_.begin(); }
+    iterator end() { return subshapes_.end(); }
+
+    [[nodiscard]] const std::vector<std::shared_ptr<Shape>>& subshapes() const {
+        return subshapes_;
+    }
+
+    [[nodiscard]] const rj_geometry::ShapeSet drawshapes() const { return drawshapes_; }
+
+    std::shared_ptr<Shape> operator[](unsigned int index) { return subshapes_[index]; }
+
+    std::shared_ptr<const Shape> operator[](unsigned int index) const { return subshapes_[index]; }
+
+    template <typename T>
+    [[nodiscard]] bool hit(const T& obj) const {
+        for (const auto& it : *this) {
+            if (it->hit(obj)) {
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    [[nodiscard]] bool hit(Point pt) const override { return hit<Point>(pt); }
+
+    [[nodiscard]] bool hit(const Segment& seg) const override { return hit<Segment>(seg); }
+
+    std::string to_string() override {
+        std::stringstream str;
+        str << "StadiumShape<";
+        for (auto& subshape : subshapes_) {
+            str << subshape->to_string() << ", ";
+        }
+        str << ">";
+
+        return str.str();
+    }
+
+protected:
+    void init(Point c1, Point c2, float r);
+
+private:
+    std::vector<std::shared_ptr<Shape>> subshapes_;
+    rj_geometry::ShapeSet drawshapes_;
+};
+
+}  // namespace rj_geometry

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,55 @@
+#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/planning/planner/plan_request.cpp

@@ -78,6 +78,23 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static,
         }
 
         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()};
+
+            std::shared_ptr<rj_geometry::StadiumShape> track_obs_ptr =
+                std::make_shared<rj_geometry::StadiumShape>(stadium);
+
+            out_static->add(track_obs_ptr);
+
+            if (in.debug_drawer != nullptr) {
+                QColor draw_color = Qt::red;
+                in.debug_drawer->draw_stadium(stadium, draw_color);
+            }
+        }
     }
 }
 

soccer/src/soccer/planning/primitives/create_path.cpp

@@ -110,7 +110,6 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst
              t += intermediate::PARAM_step_size) {
             rj_geometry::Point intermediate =
                 (final_inter - start.position).normalized(t) + start.position;
-
             auto offset = intermediate - field_dimensions->center_point();
 
             // Ignore out-of-bounds intermediate points
@@ -120,7 +119,6 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst
                 abs(offset.y()) > field_dimensions->length() / 2 + 0.2) {
                 continue;
             }
-
             Trajectory trajectory =
                 CreatePath::simple(start, goal, motion_constraints, start_time, {intermediate});
 

soccer/src/soccer/ros_debug_drawer.hpp

@@ -58,6 +58,11 @@ class RosDebugDrawer {
                 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>()