diff --git a/rj_common/include/rj_common/field_dimensions.hpp b/rj_common/include/rj_common/field_dimensions.hpp index 14a33626eef..3278e2e8be3 100644 --- a/rj_common/include/rj_common/field_dimensions.hpp +++ b/rj_common/include/rj_common/field_dimensions.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include using namespace std; diff --git a/rj_geometry/include/rj_geometry/stadium_shape.hpp b/rj_geometry/include/rj_geometry/stadium_shape.hpp new file mode 100644 index 00000000000..36cc8da7000 --- /dev/null +++ b/rj_geometry/include/rj_geometry/stadium_shape.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include + +#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 itr_shape = std::shared_ptr(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>::const_iterator; + using iterator = std::vector>::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>& subshapes() const { + return subshapes_; + } + + [[nodiscard]] const rj_geometry::ShapeSet drawshapes() const { return drawshapes_; } + + std::shared_ptr operator[](unsigned int index) { return subshapes_[index]; } + + std::shared_ptr operator[](unsigned int index) const { return subshapes_[index]; } + + template + [[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(pt); } + + [[nodiscard]] bool hit(const Segment& seg) const override { return hit(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> subshapes_; + rj_geometry::ShapeSet drawshapes_; +}; + +} // namespace rj_geometry \ No newline at end of file diff --git a/rj_geometry/src/CMakeLists.txt b/rj_geometry/src/CMakeLists.txt index af0ba6918c9..91975b6527f 100644 --- a/rj_geometry/src/CMakeLists.txt +++ b/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 diff --git a/rj_geometry/src/stadium_shape.cpp b/rj_geometry/src/stadium_shape.cpp new file mode 100644 index 00000000000..ab4cbb8bc4c --- /dev/null +++ b/rj_geometry/src/stadium_shape.cpp @@ -0,0 +1,55 @@ +#include + +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(r)}; + rj_geometry::Circle second_circle = rj_geometry::Circle{c2, static_cast(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 c1_obs_ptr = + std::make_shared(first_circle); + std::shared_ptr rect_obs_ptr = + std::make_shared(rect_obs); + std::shared_ptr c2_obs_ptr = + std::make_shared(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 \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index 09a7c225ebe..641ecef5409 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/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(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 track_obs_ptr = + std::make_shared(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); + } + } } } diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index c37c901887e..3571e4b31f7 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/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}); diff --git a/soccer/src/soccer/ros_debug_drawer.hpp b/soccer/src/soccer/ros_debug_drawer.hpp index 5c2e95b1c8b..7b1f3ac76e4 100644 --- a/soccer/src/soccer/ros_debug_drawer.hpp +++ b/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()