From a48f044c809124ae59395becbbd38cc366658047 Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 1 Oct 2024 21:46:01 -0400 Subject: [PATCH 01/16] added some code for obstacles, has bugs, will fix next time --- .../soccer/planning/planner/plan_request.cpp | 47 ++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index 09a7c225ebe..d03c6a9d088 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -74,10 +74,55 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, // Draw ball obstacle in simulator if (in.debug_drawer != nullptr) { QColor draw_color = Qt::red; - in.debug_drawer->draw_circle(ball_obs, draw_color); + // in.debug_drawer->draw_circle(ball_obs, draw_color); } 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()) { + rj_geometry::Point bp_point = maybe_bp_point.value(); + auto ball_obs2 = make_inflated_static_obs(bp_point, in.world_state->ball.velocity, kBallRadius + kAvoidBallDistance); + double x1 = ball_obs.center.x(); + double y1 = ball_obs.center.y(); + double x2 = ball_obs2.center.x(); + double y2 = ball_obs2.center.y(); + + double rect_x_left = x1 - (y2 - y1) / (2 * ball_obs.radius()); + double rect_y_left = y1 + (x2 - x1) / (2 * ball_obs.radius()); + + double rect_x_right = x2 + (y2 - y1) / (2 * ball_obs.radius()); + double rect_y_right = y2 - (x2 - x1) / (2 * ball_obs2.radius()); + + rj_geometry::Point rect_top_left{rect_x_left, rect_y_left}; + rj_geometry::Point rect_bottom_right{rect_x_right, rect_y_right}; + + rj_geometry::Rect rect_obs{rect_top_left, rect_bottom_right}; + + rj_geometry::CompositeShape track_obs{}; + std::shared_ptr ball_obs_ptr = std::make_shared(ball_obs); + std::shared_ptr rect_obs_ptr = std::make_shared(rect_obs); + std::shared_ptr ball_obs2_ptr = std::make_shared(ball_obs2); + track_obs.add(ball_obs_ptr); + track_obs.add(rect_obs_ptr); + track_obs.add(ball_obs2_ptr); + + out_static->add(std::make_shared(std::move(track_obs))); + + if (in.debug_drawer != nullptr) { + QColor draw_color = Qt::red; + in.debug_drawer->draw_circle(ball_obs, draw_color); + in.debug_drawer->draw_rect(rect_obs, draw_color); + in.debug_drawer->draw_circle(ball_obs2, draw_color); + } + } + + // replace with composite shape: 2 circles + rectangle + // circle 1: center of ball position, radius is kBallRadius + kAvoidBallDistance + // rectangle 1: top left of robot is (x1 - (y2 - y1)/(2 * (ball_obs.radius())), y1 + (x2 - x1)/(2 * (ball_obs.radius()))) + // bottom right of the robot is (x2 + (y2 - y1)/(2 * (ball_obs.radius())), y2 - (x2 - x1)/(2 * (ball_obs.radius()))) + // (x1, y1) is ball position, (x2, y2) is ball placement position + // circle 2: center of ball placement position, radius is kBallRadius + kAvoidBallDistance } } From 09b4da6917a9cb553694555c98e00cabed4b2de1 Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 8 Oct 2024 20:58:20 -0400 Subject: [PATCH 02/16] created the right shape. now just need to make it an obstacle properly so that robots don't go through it --- .../soccer/planning/planner/plan_request.cpp | 46 ++++++++++++++----- 1 file changed, 34 insertions(+), 12 deletions(-) diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index d03c6a9d088..82246002b5b 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -83,36 +83,58 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, if (maybe_bp_point.has_value()) { rj_geometry::Point bp_point = maybe_bp_point.value(); auto ball_obs2 = make_inflated_static_obs(bp_point, in.world_state->ball.velocity, kBallRadius + kAvoidBallDistance); + ball_obs2.radius(ball_obs2.radius() + in.min_dist_from_ball); double x1 = ball_obs.center.x(); double y1 = ball_obs.center.y(); double x2 = ball_obs2.center.x(); double y2 = ball_obs2.center.y(); - double rect_x_left = x1 - (y2 - y1) / (2 * ball_obs.radius()); - double rect_y_left = y1 + (x2 - x1) / (2 * ball_obs.radius()); + rj_geometry::Segment vect{in.world_state->ball.position, bp_point}; + + rj_geometry::Point end1{in.world_state->ball.position.x() + ball_obs.radius() * (bp_point.x() - in.world_state->ball.position.x()) / vect.length(), in.world_state->ball.position.y() + ball_obs.radius() * (bp_point.y() - in.world_state->ball.position.y()) / vect.length()}; + rj_geometry::Point end2{bp_point.x() - ball_obs.radius() * (bp_point.x() - in.world_state->ball.position.x()) / vect.length(), bp_point.y() - ball_obs.radius() * (bp_point.y() - in.world_state->ball.position.y()) / vect.length()}; + + rj_geometry::Segment vect_updated{end1, end2}; + rj_geometry::Polygon rect_obs{vect_updated, ball_obs.radius()}; - double rect_x_right = x2 + (y2 - y1) / (2 * ball_obs.radius()); - double rect_y_right = y2 - (x2 - x1) / (2 * ball_obs2.radius()); + // double dx = x2 - x1; + // double dy = y2 - y1; - rj_geometry::Point rect_top_left{rect_x_left, rect_y_left}; - rj_geometry::Point rect_bottom_right{rect_x_right, rect_y_right}; + // double Mx = (x1 + x2) / 2; + // double My = (y1 + y2) / 2; - rj_geometry::Rect rect_obs{rect_top_left, rect_bottom_right}; + // double l = (sqrt(dx * dx + dy * dy)); + + // double px = -dy / l; + // double py = dx / l; + + // double rect_x_left = x1 - ball_obs.radius() * px; + // double rect_y_left = y1 - ball_obs.radius() * py; + + // double rect_x_right = x2 + ball_obs.radius() * px; + // double rect_y_right = y2 + ball_obs.radius() * py; + + // rj_geometry::Point rect_top_left{rect_x_left, rect_y_left}; + // rj_geometry::Point rect_bottom_right{rect_x_right, rect_y_right}; + + // rj_geometry::Rect rect_obs{rect_top_left, rect_bottom_right}; rj_geometry::CompositeShape track_obs{}; - std::shared_ptr ball_obs_ptr = std::make_shared(ball_obs); - std::shared_ptr rect_obs_ptr = std::make_shared(rect_obs); - std::shared_ptr ball_obs2_ptr = std::make_shared(ball_obs2); + std::shared_ptr ball_obs_ptr = std::make_shared(ball_obs); + std::shared_ptr rect_obs_ptr = std::make_shared(rect_obs); + std::shared_ptr ball_obs2_ptr = std::make_shared(ball_obs2); track_obs.add(ball_obs_ptr); track_obs.add(rect_obs_ptr); track_obs.add(ball_obs2_ptr); - out_static->add(std::make_shared(std::move(track_obs))); + std::shared_ptr track_obs_ptr = std::make_shared(track_obs); + + // out_static->add(std::make_shared(track_obs)); if (in.debug_drawer != nullptr) { QColor draw_color = Qt::red; in.debug_drawer->draw_circle(ball_obs, draw_color); - in.debug_drawer->draw_rect(rect_obs, draw_color); + in.debug_drawer->draw_polygon(rect_obs, draw_color); in.debug_drawer->draw_circle(ball_obs2, draw_color); } } From 4f645bc081cc343daf69c4b70031c10072c3ea0c Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 20 Oct 2024 20:13:14 -0400 Subject: [PATCH 03/16] added the stadium shape obstacle through composite shape. next step is to make that into its own class --- soccer/src/soccer/planning/planner/plan_request.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index 82246002b5b..1695f5b5322 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -129,7 +129,7 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, std::shared_ptr track_obs_ptr = std::make_shared(track_obs); - // out_static->add(std::make_shared(track_obs)); + out_static->add(std::make_shared(track_obs)); if (in.debug_drawer != nullptr) { QColor draw_color = Qt::red; From 50b7aee5d721dbe354465ae0a07ded9072ceed69 Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 20 Oct 2024 21:24:34 -0400 Subject: [PATCH 04/16] trying to add stadium shape. structure is built, now need to implement the drawer. Untested --- .../include/rj_common/field_dimensions.hpp | 1 + .../include/rj_geometry/stadium_shape.hpp | 42 +++++++++++++++++ rj_geometry/src/CMakeLists.txt | 1 + rj_geometry/src/stadium_shape.cpp | 46 +++++++++++++++++++ soccer/src/soccer/debug_drawer.cpp | 6 +++ soccer/src/soccer/debug_drawer.hpp | 5 ++ .../soccer/planning/planner/plan_request.cpp | 28 +---------- 7 files changed, 102 insertions(+), 27 deletions(-) create mode 100644 rj_geometry/include/rj_geometry/stadium_shape.hpp create mode 100644 rj_geometry/src/stadium_shape.cpp diff --git a/rj_common/include/rj_common/field_dimensions.hpp b/rj_common/include/rj_common/field_dimensions.hpp index 14a33626eef..60ba07abd55 100644 --- a/rj_common/include/rj_common/field_dimensions.hpp +++ b/rj_common/include/rj_common/field_dimensions.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include 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..5efd9a899f8 --- /dev/null +++ b/rj_geometry/include/rj_geometry/stadium_shape.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "point.hpp" +#include "shape.hpp" +#include "segment.hpp" +#include "polygon.hpp" +#include +#include +#include + +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>& subshapes() const { + return subshapes_; + } + +protected: + void init(Point c1, Point c2, float r); + +private: + std::vector> subshapes_; +}; + +} \ 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..7a335473f5e --- /dev/null +++ b/rj_geometry/src/stadium_shape.cpp @@ -0,0 +1,46 @@ +#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); +} + +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/debug_drawer.cpp b/soccer/src/soccer/debug_drawer.cpp index f36784c88f0..0d2658caab4 100644 --- a/soccer/src/soccer/debug_drawer.cpp +++ b/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.subshapes()[0], qc, layer); +// this->draw_polygon(stadium.subshapes()[1], qc, layer); +// this->draw_circle(stadium.subshapes()[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)); diff --git a/soccer/src/soccer/debug_drawer.hpp b/soccer/src/soccer/debug_drawer.hpp index 97a35245147..582b001fe01 100644 --- a/soccer/src/soccer/debug_drawer.hpp +++ b/soccer/src/soccer/debug_drawer.hpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -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. diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index 1695f5b5322..263f33c163d 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -74,7 +74,7 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, // Draw ball obstacle in simulator if (in.debug_drawer != nullptr) { QColor draw_color = Qt::red; - // in.debug_drawer->draw_circle(ball_obs, draw_color); + in.debug_drawer->draw_circle(ball_obs, draw_color); } out_static->add(std::make_shared(std::move(ball_obs))); @@ -84,10 +84,6 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, rj_geometry::Point bp_point = maybe_bp_point.value(); auto ball_obs2 = make_inflated_static_obs(bp_point, in.world_state->ball.velocity, kBallRadius + kAvoidBallDistance); ball_obs2.radius(ball_obs2.radius() + in.min_dist_from_ball); - double x1 = ball_obs.center.x(); - double y1 = ball_obs.center.y(); - double x2 = ball_obs2.center.x(); - double y2 = ball_obs2.center.y(); rj_geometry::Segment vect{in.world_state->ball.position, bp_point}; @@ -97,28 +93,6 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, rj_geometry::Segment vect_updated{end1, end2}; rj_geometry::Polygon rect_obs{vect_updated, ball_obs.radius()}; - // double dx = x2 - x1; - // double dy = y2 - y1; - - // double Mx = (x1 + x2) / 2; - // double My = (y1 + y2) / 2; - - // double l = (sqrt(dx * dx + dy * dy)); - - // double px = -dy / l; - // double py = dx / l; - - // double rect_x_left = x1 - ball_obs.radius() * px; - // double rect_y_left = y1 - ball_obs.radius() * py; - - // double rect_x_right = x2 + ball_obs.radius() * px; - // double rect_y_right = y2 + ball_obs.radius() * py; - - // rj_geometry::Point rect_top_left{rect_x_left, rect_y_left}; - // rj_geometry::Point rect_bottom_right{rect_x_right, rect_y_right}; - - // rj_geometry::Rect rect_obs{rect_top_left, rect_bottom_right}; - rj_geometry::CompositeShape track_obs{}; std::shared_ptr ball_obs_ptr = std::make_shared(ball_obs); std::shared_ptr rect_obs_ptr = std::make_shared(rect_obs); From 19504d89dd9cfd9dc76cdbb5ac27d2248d89e5ac Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 22 Oct 2024 20:38:05 -0400 Subject: [PATCH 05/16] stadium shape is fully working and has been implemented into plan request. for some reason, we must use the shape set stadium has instead of make stadium a shared pointer to add it to our static obstacles. drawing the stadium shape is also working correctly. the stadium shape will only be added as an obstacle if it is their ball placement. --- .../include/rj_geometry/stadium_shape.hpp | 6 +++ rj_geometry/src/stadium_shape.cpp | 4 ++ soccer/src/soccer/debug_drawer.cpp | 6 +-- soccer/src/soccer/debug_drawer.hpp | 6 +-- .../soccer/planning/planner/plan_request.cpp | 37 +++---------------- soccer/src/soccer/ros_debug_drawer.hpp | 4 ++ 6 files changed, 26 insertions(+), 37 deletions(-) diff --git a/rj_geometry/include/rj_geometry/stadium_shape.hpp b/rj_geometry/include/rj_geometry/stadium_shape.hpp index 5efd9a899f8..dba27e92ebf 100644 --- a/rj_geometry/include/rj_geometry/stadium_shape.hpp +++ b/rj_geometry/include/rj_geometry/stadium_shape.hpp @@ -4,6 +4,7 @@ #include "shape.hpp" #include "segment.hpp" #include "polygon.hpp" +#include "shape_set.hpp" #include #include #include @@ -32,11 +33,16 @@ class StadiumShape : public Shape { return subshapes_; } + [[nodiscard]] const rj_geometry::ShapeSet drawshapes() const { + return drawshapes_; + } + protected: void init(Point c1, Point c2, float r); private: std::vector> subshapes_; + rj_geometry::ShapeSet drawshapes_; }; } \ No newline at end of file diff --git a/rj_geometry/src/stadium_shape.cpp b/rj_geometry/src/stadium_shape.cpp index 7a335473f5e..3c01aedc2ec 100644 --- a/rj_geometry/src/stadium_shape.cpp +++ b/rj_geometry/src/stadium_shape.cpp @@ -23,6 +23,10 @@ void StadiumShape::init(Point c1, Point c2, float r) { 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 { diff --git a/soccer/src/soccer/debug_drawer.cpp b/soccer/src/soccer/debug_drawer.cpp index 0d2658caab4..1c34cc09035 100644 --- a/soccer/src/soccer/debug_drawer.cpp +++ b/soccer/src/soccer/debug_drawer.cpp @@ -49,9 +49,9 @@ void DebugDrawer::draw_circle(rj_geometry::Point center, float radius, const QCo } // void DebugDrawer::draw_stadium(rj_geometry::StadiumShape& stadium, const QColor& qc, const QString& layer) { -// this->draw_circle(stadium.subshapes()[0], qc, layer); -// this->draw_polygon(stadium.subshapes()[1], qc, layer); -// this->draw_circle(stadium.subshapes()[2], qc, 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) { diff --git a/soccer/src/soccer/debug_drawer.hpp b/soccer/src/soccer/debug_drawer.hpp index 582b001fe01..495d8e9c73a 100644 --- a/soccer/src/soccer/debug_drawer.hpp +++ b/soccer/src/soccer/debug_drawer.hpp @@ -79,9 +79,9 @@ 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()); + // 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, diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index 263f33c163d..648576e2568 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -80,45 +80,20 @@ 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()) { + if (maybe_bp_point.has_value() && in.play_state.is_their_restart()) { rj_geometry::Point bp_point = maybe_bp_point.value(); - auto ball_obs2 = make_inflated_static_obs(bp_point, in.world_state->ball.velocity, kBallRadius + kAvoidBallDistance); - ball_obs2.radius(ball_obs2.radius() + in.min_dist_from_ball); + rj_geometry::StadiumShape stadium = rj_geometry::StadiumShape{in.world_state->ball.position, bp_point, ball_obs.radius()}; - rj_geometry::Segment vect{in.world_state->ball.position, bp_point}; - - rj_geometry::Point end1{in.world_state->ball.position.x() + ball_obs.radius() * (bp_point.x() - in.world_state->ball.position.x()) / vect.length(), in.world_state->ball.position.y() + ball_obs.radius() * (bp_point.y() - in.world_state->ball.position.y()) / vect.length()}; - rj_geometry::Point end2{bp_point.x() - ball_obs.radius() * (bp_point.x() - in.world_state->ball.position.x()) / vect.length(), bp_point.y() - ball_obs.radius() * (bp_point.y() - in.world_state->ball.position.y()) / vect.length()}; - - rj_geometry::Segment vect_updated{end1, end2}; - rj_geometry::Polygon rect_obs{vect_updated, 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 track_obs_ptr = std::make_shared(stadium); - rj_geometry::CompositeShape track_obs{}; - std::shared_ptr ball_obs_ptr = std::make_shared(ball_obs); - std::shared_ptr rect_obs_ptr = std::make_shared(rect_obs); - std::shared_ptr ball_obs2_ptr = std::make_shared(ball_obs2); - track_obs.add(ball_obs_ptr); - track_obs.add(rect_obs_ptr); - track_obs.add(ball_obs2_ptr); - - std::shared_ptr track_obs_ptr = std::make_shared(track_obs); - - out_static->add(std::make_shared(track_obs)); + out_static->add(stadium.drawshapes()); if (in.debug_drawer != nullptr) { QColor draw_color = Qt::red; - in.debug_drawer->draw_circle(ball_obs, draw_color); - in.debug_drawer->draw_polygon(rect_obs, draw_color); - in.debug_drawer->draw_circle(ball_obs2, draw_color); + in.debug_drawer->draw_stadium(stadium, draw_color); } } - - // replace with composite shape: 2 circles + rectangle - // circle 1: center of ball position, radius is kBallRadius + kAvoidBallDistance - // rectangle 1: top left of robot is (x1 - (y2 - y1)/(2 * (ball_obs.radius())), y1 + (x2 - x1)/(2 * (ball_obs.radius()))) - // bottom right of the robot is (x2 + (y2 - y1)/(2 * (ball_obs.radius())), y2 - (x2 - x1)/(2 * (ball_obs.radius()))) - // (x1, y1) is ball position, (x2, y2) is ball placement position - // circle 2: center of ball placement position, radius is kBallRadius + kAvoidBallDistance } } diff --git a/soccer/src/soccer/ros_debug_drawer.hpp b/soccer/src/soccer/ros_debug_drawer.hpp index 5c2e95b1c8b..c22f477f635 100644 --- a/soccer/src/soccer/ros_debug_drawer.hpp +++ b/soccer/src/soccer/ros_debug_drawer.hpp @@ -58,6 +58,10 @@ 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() From 8418efee5faa8c4560d1c70fa8f9b80a1338308f Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 29 Oct 2024 20:16:33 -0400 Subject: [PATCH 06/16] resolved changes requested in my pr --- .../include/rj_geometry/stadium_shape.hpp | 53 +++++++++++++++++++ soccer/src/soccer/debug_drawer.cpp | 6 --- soccer/src/soccer/debug_drawer.hpp | 5 -- .../soccer/planning/planner/plan_request.cpp | 5 +- 4 files changed, 55 insertions(+), 14 deletions(-) diff --git a/rj_geometry/include/rj_geometry/stadium_shape.hpp b/rj_geometry/include/rj_geometry/stadium_shape.hpp index dba27e92ebf..0513aeac1d0 100644 --- a/rj_geometry/include/rj_geometry/stadium_shape.hpp +++ b/rj_geometry/include/rj_geometry/stadium_shape.hpp @@ -24,11 +24,28 @@ class StadiumShape : public Shape { 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_; } @@ -37,6 +54,42 @@ class StadiumShape : public Shape { 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); diff --git a/soccer/src/soccer/debug_drawer.cpp b/soccer/src/soccer/debug_drawer.cpp index 1c34cc09035..f36784c88f0 100644 --- a/soccer/src/soccer/debug_drawer.cpp +++ b/soccer/src/soccer/debug_drawer.cpp @@ -48,12 +48,6 @@ 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)); diff --git a/soccer/src/soccer/debug_drawer.hpp b/soccer/src/soccer/debug_drawer.hpp index 495d8e9c73a..97a35245147 100644 --- a/soccer/src/soccer/debug_drawer.hpp +++ b/soccer/src/soccer/debug_drawer.hpp @@ -4,7 +4,6 @@ #include #include -#include #include #include #include @@ -79,10 +78,6 @@ 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. diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index 648576e2568..ec6819fce17 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -84,10 +84,9 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, 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 track_obs_ptr = std::make_shared(stadium); + std::shared_ptr track_obs_ptr = std::make_shared(stadium); - out_static->add(stadium.drawshapes()); + out_static->add(track_obs_ptr); if (in.debug_drawer != nullptr) { QColor draw_color = Qt::red; From c593823e2634c34848068b4029b35486abf515bb Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 29 Sep 2024 19:51:30 -0400 Subject: [PATCH 07/16] Intermediate Pathgen (#2266) * initial test of tigers system * clean up and add comment * added line test * fix line issue * made comments and removed from settle * Fix Code Style On intermediate-pathgen (#2267) automated style fixes Co-authored-by: sanatd33 * make pr changes * add cache inter * start cacching intermediates * add params * use abs angle * Fix Code Style On intermediate-pathgen (#2269) automated style fixes Co-authored-by: sid-parikh --------- Co-authored-by: rishiso Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: sanatd33 Co-authored-by: petergarud Co-authored-by: sid-parikh --- soccer/src/soccer/CMakeLists.txt | 1 + .../planner/line_kick_path_planner.cpp | 2 + .../planning/planner/settle_path_planner.cpp | 12 +- .../src/soccer/planning/planning_params.cpp | 13 +++ .../src/soccer/planning/planning_params.hpp | 7 ++ .../planning/primitives/create_path.cpp | 101 +++++++++++++++++ .../planning/primitives/create_path.hpp | 11 +- .../soccer/planning/primitives/replanner.cpp | 17 ++- .../soccer/strategy/agent/position/line.cpp | 105 ++++++++++++++++++ .../soccer/strategy/agent/position/line.hpp | 25 +++++ .../agent/position/robot_factory_position.hpp | 3 +- 11 files changed, 280 insertions(+), 17 deletions(-) create mode 100644 soccer/src/soccer/strategy/agent/position/line.cpp create mode 100644 soccer/src/soccer/strategy/agent/position/line.hpp diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index ed6a2c41e65..452c350b9e2 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -79,6 +79,7 @@ set(ROBOCUP_LIB_SRC strategy/agent/position/position.cpp strategy/agent/position/robot_factory_position.cpp strategy/agent/position/goalie.cpp + strategy/agent/position/line.cpp strategy/agent/position/offense.cpp strategy/agent/position/idle.cpp strategy/agent/position/defense.cpp diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp index ecbf3732a47..1faf9dbb467 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -88,6 +88,8 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { MotionCommand modified_command{"path_target", target, FacePoint{plan_request.motion_command.target.position}}; + + modified_command.ignore_ball = true; modified_request.motion_command = modified_command; return path_target_.plan(modified_request); diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index dde32594b56..6e4cf03258f 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -221,9 +221,9 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn // Plan a path from our partial path start location to the intercept // test location - Trajectory path = CreatePath::rrt(start_instant.linear_motion(), target_robot_intersection, - plan_request.constraints.mot, start_instant.stamp, - static_obstacles, dynamic_obstacles); + Trajectory path = CreatePath::intermediate( + start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot, + start_instant.stamp, static_obstacles); // Calculate the RJ::Seconds buffer_duration = ball_time - path.duration(); @@ -332,9 +332,9 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn LinearMotionInstant target{closest_pt, settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_}; - Trajectory shortcut = - CreatePath::rrt(start_instant.linear_motion(), target, plan_request.constraints.mot, - start_instant.stamp, static_obstacles, dynamic_obstacles); + Trajectory shortcut = CreatePath::intermediate(start_instant.linear_motion(), target, + plan_request.constraints.mot, + start_instant.stamp, static_obstacles); if (!shortcut.empty()) { plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos), diff --git a/soccer/src/soccer/planning/planning_params.cpp b/soccer/src/soccer/planning/planning_params.cpp index 6a6d3621fed..e94b3abba75 100644 --- a/soccer/src/soccer/planning/planning_params.cpp +++ b/soccer/src/soccer/planning/planning_params.cpp @@ -41,6 +41,19 @@ DEFINE_NS_INT64(kPlanningParamModule, rrt, min_iterations, 50, DEFINE_NS_INT64(kPlanningParamModule, rrt, max_iterations, 500, "Maximum number of RRT iterations to run before giving up"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale, 0.5, + "Minimum length for intermediate point (m)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale, 1.5, + "Maximum length for intermediate point (m)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle, 20, + "Minimum angle for intermediate point (deg)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle, 140, + "Maximum angle for intermediate point (deg)"); +DEFINE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates, 5, + "Number of intermediate points used (unitless)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size, 0.1, + "Step size for testing intermediates (m)"); + DEFINE_NS_FLOAT64( kPlanningParamModule, escape, step_size, 0.1, "Step size for the RRT used to find an unblocked point in find_non_blocked_goal()"); diff --git a/soccer/src/soccer/planning/planning_params.hpp b/soccer/src/soccer/planning/planning_params.hpp index 03a88917244..f22fe373969 100644 --- a/soccer/src/soccer/planning/planning_params.hpp +++ b/soccer/src/soccer/planning/planning_params.hpp @@ -25,6 +25,13 @@ DECLARE_NS_FLOAT64(kPlanningParamModule, rrt, waypoint_bias); DECLARE_NS_INT64(kPlanningParamModule, rrt, min_iterations); DECLARE_NS_INT64(kPlanningParamModule, rrt, max_iterations); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle); +DECLARE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size); + DECLARE_NS_FLOAT64(kPlanningParamModule, escape, step_size); DECLARE_NS_FLOAT64(kPlanningParamModule, escape, goal_change_threshold); diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 11feff152d7..d510483a0ad 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,4 +75,105 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } +static std::optional> cached_intermediate_tuple_{}; + +Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, + const MotionConstraints& motion_constraints, RJ::Time start_time, + const rj_geometry::ShapeSet& static_obstacles) { + // if already on goal, no need to move + if (start.position.dist_to(goal.position) < 1e-6) { + return Trajectory{{RobotInstant{Pose(start.position, 0), Twist(), start_time}}}; + } + + // maybe straight line works + Trajectory straight_trajectory = + CreatePath::simple(start, goal, motion_constraints, start_time); + + // If we are very close to the goal (i.e. there physically can't be a robot + // in our way) or the straight trajectory is feasible, we can use it. + if (start.position.dist_to(goal.position) < kRobotRadius || + (!trajectory_hits_static(straight_trajectory, static_obstacles, start_time, nullptr))) { + return straight_trajectory; + } + + // Generate list of intermediate points + std::vector intermediates = get_intermediates(start, goal); + + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { + rj_geometry::Point final_inter = intermediates[i]; + + // Step through the path from the robot to the final intermediate point + // and test each point on that path as an intermediate point + for (double t = intermediate::PARAM_step_size; t < final_inter.dist_to(start.position); + t += intermediate::PARAM_step_size) { + rj_geometry::Point intermediate = + (final_inter - start.position).normalized(t) + start.position; + Trajectory trajectory = + CreatePath::simple(start, goal, motion_constraints, start_time, {intermediate}); + + // If the trajectory does not hit an obstacle, it is valid + if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) { + auto angle = (final_inter - start.position).angle(); + cached_intermediate_tuple_ = {abs(angle), signbit(angle) ? -1 : 1, + (final_inter - start.position).mag()}; + return trajectory; + } + } + } + + // If all else fails, return the straight-line trajectory + return straight_trajectory; +} + +std::vector get_intermediates(const LinearMotionInstant& start, + const LinearMotionInstant& goal) { + std::random_device rd; + std::mt19937 gen(rd()); + // Create a random distribution for the distance between the start + // and the intermediate points + std::uniform_real_distribution<> scale_dist(intermediate::PARAM_min_scale, + intermediate::PARAM_max_scale); + + double angle_range = intermediate::PARAM_max_angle - intermediate::PARAM_min_angle; + // Create a random distribution for the angle between the start + // and the intermediate points + std::uniform_real_distribution<> angle_dist(-angle_range, angle_range); + + std::vector intermediates; + std::vector> inter_tuples; + + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { + double angle = angle_dist(gen); + angle += std::copysign(intermediate::PARAM_min_angle, angle); + angle = degrees_to_radians(angle); + double scale = scale_dist(gen); + + // Generate random tuples of distances and angles + inter_tuples.emplace_back(abs(angle), signbit(angle) ? -1 : 1, scale); + } + + if (cached_intermediate_tuple_) { + inter_tuples.push_back(*cached_intermediate_tuple_); + } + + // Sort the list of tuples by the magnitude of angle + // This ensures that we take paths with + // smaller offsets from the simple path + sort(inter_tuples.begin(), inter_tuples.end()); + + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { + double angle = std::get<0>(inter_tuples[i]) * std::get<1>(inter_tuples[i]); + double scale = std::get<2>(inter_tuples[i]); + + double fin_angle = goal.position.angle_to(start.position) + angle; + double fin_length = scale; + + // Convert the distances and angles into a point + intermediates.push_back(start.position + rj_geometry::Point{fin_length * cos(fin_angle), + fin_length * sin(fin_angle)}); + } + + return intermediates; +} + } // namespace planning::CreatePath diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index ad73bcdc8b7..d71cf5809f8 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -1,8 +1,11 @@ #pragma once +#include +#include + #include "planning/motion_constraints.hpp" -#include "planning/trajectory.hpp" #include "planning/primitives/path_smoothing.hpp" +#include "planning/trajectory.hpp" namespace planning::CreatePath { @@ -24,4 +27,10 @@ Trajectory simple( const MotionConstraints& motion_constraints, RJ::Time start_time, const std::vector& intermediate_points = {}); +Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, + const MotionConstraints& motion_constraints, RJ::Time start_time, + const rj_geometry::ShapeSet& static_obstacles); + +std::vector get_intermediates(const LinearMotionInstant& start, + const LinearMotionInstant& goal); } // namespace planning::CreatePath \ No newline at end of file diff --git a/soccer/src/soccer/planning/primitives/replanner.cpp b/soccer/src/soccer/planning/primitives/replanner.cpp index 69da4a2e407..3c5725f5047 100644 --- a/soccer/src/soccer/planning/primitives/replanner.cpp +++ b/soccer/src/soccer/planning/primitives/replanner.cpp @@ -31,10 +31,9 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& } Trajectory pre_trajectory = partial_path(previous, params.start.stamp); - Trajectory post_trajectory = - CreatePath::rrt(pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot, - pre_trajectory.end_time(), params.static_obstacles, - params.dynamic_obstacles, bias_waypoints); + Trajectory post_trajectory = CreatePath::intermediate( + pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot, + pre_trajectory.end_time(), params.static_obstacles); // If we couldn't profile such that velocity at the end of the partial replan period is valid, // do a full replan. @@ -57,8 +56,8 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { Trajectory path = - CreatePath::rrt(params.start.linear_motion(), params.goal, params.constraints.mot, - params.start.stamp, params.static_obstacles, params.dynamic_obstacles); + CreatePath::intermediate(params.start.linear_motion(), params.goal, params.constraints.mot, + params.start.stamp, params.static_obstacles); // if the initial path is empty, the goal must be blocked // try to shift the goal_point until it is no longer blocked @@ -76,9 +75,9 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { almost_goal.position += shift_dir * shift_size; - path = - CreatePath::rrt(params.start.linear_motion(), almost_goal, params.constraints.mot, - params.start.stamp, params.static_obstacles, params.dynamic_obstacles); + path = CreatePath::intermediate(params.start.linear_motion(), almost_goal, + params.constraints.mot, params.start.stamp, + params.static_obstacles); } if (!path.empty()) { diff --git a/soccer/src/soccer/strategy/agent/position/line.cpp b/soccer/src/soccer/strategy/agent/position/line.cpp new file mode 100644 index 00000000000..daf99ef1112 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/line.cpp @@ -0,0 +1,105 @@ +#include "line.hpp" + +namespace strategy { + +Line::Line(const Position& other) : Position{other} { position_name_ = "Line"; } + +Line::Line(int r_id) : Position{r_id, "Line"} {} + +std::optional Line::derived_get_task(RobotIntent intent) { + if (check_is_done()) { + forward_ = !forward_; + } + + if (vertical_) { + if (forward_) { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1, + field_dimensions_.center_field_loc().y() - 2.5 + 5 * 0.75, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } else { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1, + field_dimensions_.center_field_loc().y() - 4.5 + 5 * 0.75, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } + } else { + if (forward_) { + if (face_target_) { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.center_field_loc().x() - 2.5 + 5 * 0.75, + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } else { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.our_defense_area().maxx(), + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceAngle{0}, true}; + + intent.motion_command = motion_command; + } + + } else { + if (face_target_) { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.our_defense_area().minx(), + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } else { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.our_defense_area().minx(), + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceAngle{0}, true}; + + intent.motion_command = motion_command; + } + } + } + + return intent; +} +} // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/line.hpp b/soccer/src/soccer/strategy/agent/position/line.hpp new file mode 100644 index 00000000000..a47b31701a6 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/line.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include "position.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace strategy { +class Line : public Position { +public: + Line(const Position& other); + Line(int r_id); + ~Line() override = default; + Line(const Line& other) = default; + Line(Line&& other) = default; + + std::string get_current_state() override { return "Line"; } + +private: + std::optional derived_get_task(RobotIntent intent) override; + bool forward_ = true; + bool vertical_ = false; + bool face_target_ = false; +}; +} // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp index a8802d828cd..aae7765c166 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -20,6 +20,7 @@ #include "strategy/agent/position/free_kicker.hpp" #include "strategy/agent/position/goal_kicker.hpp" #include "strategy/agent/position/goalie.hpp" +#include "strategy/agent/position/line.hpp" #include "strategy/agent/position/offense.hpp" #include "strategy/agent/position/penalty_non_kicker.hpp" #include "strategy/agent/position/penalty_player.hpp" @@ -146,9 +147,9 @@ class RobotFactoryPosition : public Position { // return; // } if (dynamic_cast(current_position_.get()) == nullptr) { - SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name()); // This line requires Pos to implement the constructor Pos(const // Position&) + SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name()); current_position_->die(); current_position_ = std::make_unique(*current_position_); } From 2d0b689b9745d0fbf2ef6f8027c4f0e2c60f6086 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Tue, 1 Oct 2024 09:49:50 -0400 Subject: [PATCH 08/16] Update Github Actions (#2283) * change to humble * change dockerfile * remvoe werror switch --------- Co-authored-by: petergarud --- .github/workflows/ci.yml | 10 +++++----- CMakeLists.txt | 2 +- Dockerfile | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 75bbceb0a46..41256d6d37d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -7,7 +7,7 @@ jobs: build-and-test: runs-on: ubuntu-latest container: - image: robojackets/robocup-software:foxy + image: robojackets/robocup-software:humble defaults: run: shell: bash @@ -34,7 +34,7 @@ jobs: - name: Build run: | echo "::add-matcher::ci/clang.json" - source /opt/ros/foxy/setup.bash + source /opt/ros/humble/setup.bash source install/setup.bash make all echo "::remove-matcher owner=clang::" @@ -42,7 +42,7 @@ jobs: - name: Test run: | echo "::add-matcher::ci/gtest.json" - source /opt/ros/foxy/setup.bash + source /opt/ros/humble/setup.bash source install/setup.bash ./install/lib/rj_robocup/test-soccer echo "::remove-matcher owner=gtest::" @@ -50,7 +50,7 @@ jobs: - name: Run clang-tidy run: | echo "::add-matcher::ci/clang-tidy.json" - source /opt/ros/foxy/setup.bash + source /opt/ros/humble/setup.bash source install/setup.bash DIFFBASE=${{ github.base_ref }} make checktidy-lines echo "::remove-matcher owner=clang-tidy::" @@ -61,7 +61,7 @@ jobs: if: startsWith(github.head_ref, 'fix-code-style') == false runs-on: ubuntu-latest container: - image: ros:foxy + image: ros:humble defaults: run: shell: bash diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c567db967a..d41a091bde7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,7 +78,7 @@ endif() set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O2 -march=native") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Og") set(CMAKE_CXX_FLAGS_DEBUG - "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor -Werror=switch") + "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor") # ====================================================================== # Testing diff --git a/Dockerfile b/Dockerfile index f1bdef3446b..d4d075085b3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,5 +1,5 @@ # Use phusion/baseimage if problems arise -FROM ros:foxy-ros-base-focal +FROM ros:humble-ros-base-jammy LABEL maintainer="oswinso@gmail.com" # Setup apt to be happy with no console input From 5314aa9de818185af484cc35d4f23af33b78beb0 Mon Sep 17 00:00:00 2001 From: Aalind Tyagi <42896179+Squid5678@users.noreply.github.com> Date: Tue, 1 Oct 2024 19:18:05 -0400 Subject: [PATCH 09/16] Remove Dependency on Deprecated Disutils (#2281) updated files --- install/setup.bash | 2 +- install/setup.zsh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/install/setup.bash b/install/setup.bash index b84bbe02bab..6e2b3722020 100755 --- a/install/setup.bash +++ b/install/setup.bash @@ -34,7 +34,7 @@ _pythonpath_add() { # No direct replacement for the command exists, and in Ubuntu 22.04, # this script will produce a warning about this. We may need to find a # replacement library in the future. -_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))") +_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))") _pythonpath_add "${_PYTHON_LIB_PATH}" unset _PYTHON_LIB_PATH diff --git a/install/setup.zsh b/install/setup.zsh index 4d6b7f16a21..db45a72c353 100644 --- a/install/setup.zsh +++ b/install/setup.zsh @@ -30,7 +30,7 @@ _pythonpath_add() { fi } -_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))") +_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))") _pythonpath_add "${_PYTHON_LIB_PATH}" unset _PYTHON_LIB_PATH From 079fb728cfdb6f59fd2aa328eb6cbdea2640e4c3 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Tue, 1 Oct 2024 20:50:53 -0400 Subject: [PATCH 10/16] Dribble in Sim (#2287) * dribbler is stupid * use realism * add realism to simoulator cli run --------- Co-authored-by: petergarud --- launch/framework.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/framework.bash b/launch/framework.bash index bf67387d40a..b5dcc9e9f77 100755 --- a/launch/framework.bash +++ b/launch/framework.bash @@ -10,7 +10,7 @@ if [ -z "$binary" ]; then fi # Run the binary in the background -"$binary" & +"$binary" -g 2020B --realism RC2021 & binary_pid=$! # Ensure that pressing Ctrl+C kills all subprocesses From 27cb3d6b4e55134344853b304db21a20da21425e Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 6 Oct 2024 19:00:27 -0400 Subject: [PATCH 11/16] add ros env vars to source script (#2288) --- .gitignore | 1 - install/env.sh | 5 +++++ makefile | 6 +++--- source.bash | 3 +++ 4 files changed, 11 insertions(+), 4 deletions(-) create mode 100644 install/env.sh diff --git a/.gitignore b/.gitignore index 0954366ef7e..176668b4431 100644 --- a/.gitignore +++ b/.gitignore @@ -6,7 +6,6 @@ # build directories /build -/install /gmon.out *.swp /.settings diff --git a/install/env.sh b/install/env.sh new file mode 100644 index 00000000000..e9bc1810806 --- /dev/null +++ b/install/env.sh @@ -0,0 +1,5 @@ +# Here are some useful environment variables for setting up a consistent ROS environment +# some information on this can be found here: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html +export ROS_DOMAIN_ID=19 # this is a random number, it's just important to keep it consistent across shells +export ROS_LOCALHOST_ONLY=1 # for our stack specifically, this is quite helpful. + diff --git a/makefile b/makefile index 4ddb5959fb1..837c56b2949 100644 --- a/makefile +++ b/makefile @@ -72,11 +72,11 @@ run-sim: # run our stack with default flags # TODO: actually name our software stack something run-our-stack: - ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True + ros2 launch rj_robocup soccer.launch.py run_sim:=True # run sim with external referee (SSL Game Controller) run-sim-external: - ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False + ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False run-sim-ex: run-sim-external @@ -95,7 +95,7 @@ run-manual: # same as run-real, with different server port run-alt-real: - ROS_DOMAIN_ID=2 ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b + ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b # run sim2play (requires external referee) run-sim2play: diff --git a/source.bash b/source.bash index 85a5e0bedf1..d86ad19995f 100755 --- a/source.bash +++ b/source.bash @@ -9,3 +9,6 @@ if [[ $SHELL == *"zsh"* ]]; then source /opt/ros/humble/setup.zsh source install/setup.zsh fi + +source install/env.sh + From 76cbe712ae6fb8f06b8a1086edd3293400528bc7 Mon Sep 17 00:00:00 2001 From: Jack <65086686+jacksherling@users.noreply.github.com> Date: Sun, 6 Oct 2024 19:02:08 -0400 Subject: [PATCH 12/16] Update tutorial.rst (#2286) --- docs/source/tutorial.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/source/tutorial.rst b/docs/source/tutorial.rst index b44e331c40e..d836f27c95e 100644 --- a/docs/source/tutorial.rst +++ b/docs/source/tutorial.rst @@ -351,6 +351,7 @@ but here are some more hints. * The motion command for driving in a straight line is :cpp:`"path_target"`. * You will probably need to override some methods relating to passing, but you can leave their implementations empty. They don't need to do anything in your position, as your robot will not pass the ball * The simulator tells you the coordinates of your cursor—these are the same coordinates you can use in your motion commands. +* You will need to add the new file name you create to ``soccer/src/soccer/CMakeLists.txt``. See how this is done for other positions. Testing ~~~~~~~ @@ -668,4 +669,4 @@ Here are all the external links from this document, copied again for your easy r * `C++ Inheritance`_ -.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/ \ No newline at end of file +.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/ From 091a744af05502da3780777bdd8634fe536751da Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 27 Oct 2024 20:05:23 -0400 Subject: [PATCH 13/16] Fix the Fix Style (#2296) change to clang format 12 --- .github/workflows/ci.yml | 2 +- util/clang-format-diff.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 41256d6d37d..3290c698776 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-10 -i -p1 + git diff -U0 --no-color ${{ github.base_ref }} ${{ github.head_ref }} | python3 util/clang-format-diff.py -binary clang-format-12 -i -p1 if ! git diff-index --quiet HEAD; then echo "::set-output name=changed::true" diff --git a/util/clang-format-diff.py b/util/clang-format-diff.py index 2a91ef4992f..722796faf3d 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-10", + default="clang-format-12", help="location of binary to use for clang-format", ) args = parser.parse_args() From ee9d3b20d028838122d4f13088dec4c2477e5e85 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 27 Oct 2024 20:31:03 -0400 Subject: [PATCH 14/16] Waller Planner (#2293) * start making wall planner * asdf1234 * complete waller planner * fix the fix the fix style --- .github/workflows/ci.yml | 2 +- .../strategy/agent/position/defense.cpp | 4 +- .../soccer/strategy/agent/position/waller.cpp | 80 ++++++++++++------- .../soccer/strategy/agent/position/waller.hpp | 7 +- util/clang-format-diff.py | 2 +- 5 files changed, 56 insertions(+), 39 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3290c698776..9392d80fe5c 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 cf8836c01f0..7f91db23882 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 6919cdb7eb6..c841f7dc4d1 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 ab06c8b7d56..41a3bf3a6bd 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 722796faf3d..1c6e48457c5 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() From 664d8fc46644e505e788d172bea1d3b9c7cd9a56 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 29 Oct 2024 20:20:01 -0400 Subject: [PATCH 15/16] Fix Code Style On ball-placement-obstacle (#2304) automated style fixes Co-authored-by: shourikb --- .../include/rj_common/field_dimensions.hpp | 2 +- .../include/rj_geometry/stadium_shape.hpp | 40 ++++++++----------- rj_geometry/src/stadium_shape.cpp | 19 +++++---- .../soccer/planning/planner/plan_request.cpp | 6 ++- soccer/src/soccer/ros_debug_drawer.hpp | 3 +- 5 files changed, 35 insertions(+), 35 deletions(-) diff --git a/rj_common/include/rj_common/field_dimensions.hpp b/rj_common/include/rj_common/field_dimensions.hpp index 60ba07abd55..3278e2e8be3 100644 --- a/rj_common/include/rj_common/field_dimensions.hpp +++ b/rj_common/include/rj_common/field_dimensions.hpp @@ -7,13 +7,13 @@ #include #include #include -#include #include #include #include #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 index 0513aeac1d0..36cc8da7000 100644 --- a/rj_geometry/include/rj_geometry/stadium_shape.hpp +++ b/rj_geometry/include/rj_geometry/stadium_shape.hpp @@ -1,18 +1,20 @@ #pragma once +#include +#include +#include + #include "point.hpp" -#include "shape.hpp" -#include "segment.hpp" #include "polygon.hpp" +#include "segment.hpp" +#include "shape.hpp" #include "shape_set.hpp" -#include -#include -#include 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. + * 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: @@ -20,9 +22,7 @@ class StadiumShape : public Shape { StadiumShape() = default; - StadiumShape(Point c1, Point c2, float r) { - init(c1, c2, r); - } + StadiumShape(Point c1, Point c2, float r) { init(c1, c2, r); } StadiumShape(const StadiumShape& other) { for (const auto& shape : other.subshapes_) { @@ -50,19 +50,13 @@ class StadiumShape : public Shape { return subshapes_; } - [[nodiscard]] const rj_geometry::ShapeSet drawshapes() const { - return drawshapes_; - } + [[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) { return subshapes_[index]; } - std::shared_ptr operator[](unsigned int index) const { - return subshapes_[index]; - } + std::shared_ptr operator[](unsigned int index) const { return subshapes_[index]; } - template + template [[nodiscard]] bool hit(const T& obj) const { for (const auto& it : *this) { if (it->hit(obj)) { @@ -75,11 +69,9 @@ class StadiumShape : public Shape { [[nodiscard]] bool hit(Point pt) const override { return hit(pt); } - [[nodiscard]] bool hit(const Segment& seg) const override { - return hit(seg); - } + [[nodiscard]] bool hit(const Segment& seg) const override { return hit(seg); } - std::string to_string() override { + std::string to_string() override { std::stringstream str; str << "StadiumShape<"; for (auto& subshape : subshapes_) { @@ -98,4 +90,4 @@ class StadiumShape : public Shape { rj_geometry::ShapeSet drawshapes_; }; -} \ No newline at end of file +} // namespace rj_geometry \ No newline at end of file diff --git a/rj_geometry/src/stadium_shape.cpp b/rj_geometry/src/stadium_shape.cpp index 3c01aedc2ec..ab4cbb8bc4c 100644 --- a/rj_geometry/src/stadium_shape.cpp +++ b/rj_geometry/src/stadium_shape.cpp @@ -9,16 +9,21 @@ void StadiumShape::init(Point c1, Point c2, float 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::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); + 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); diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index ec6819fce17..641ecef5409 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -82,9 +82,11 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, 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()}; + 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); + std::shared_ptr track_obs_ptr = + std::make_shared(stadium); out_static->add(track_obs_ptr); diff --git a/soccer/src/soccer/ros_debug_drawer.hpp b/soccer/src/soccer/ros_debug_drawer.hpp index c22f477f635..7b1f3ac76e4 100644 --- a/soccer/src/soccer/ros_debug_drawer.hpp +++ b/soccer/src/soccer/ros_debug_drawer.hpp @@ -58,7 +58,8 @@ class RosDebugDrawer { color_from_qt(color))); } - void draw_stadium(const rj_geometry::StadiumShape& stadium, const QColor& color = QColor::fromRgb(0, 0, 0, 0)) { + void draw_stadium(const rj_geometry::StadiumShape& stadium, + const QColor& color = QColor::fromRgb(0, 0, 0, 0)) { this->draw_shapes(stadium.drawshapes(), color); } From f6db77753d2dea778525bbb8c0c3c24677fbaa82 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 16 Dec 2024 00:38:26 -0500 Subject: [PATCH 16/16] Fix Code Style On ball-placement-obstacle (#2316) --- soccer/src/soccer/planning/primitives/create_path.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 8699bd3e801..3571e4b31f7 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,7 +75,6 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } - static std::unordered_map> cached_intermediate_tuple_{}; Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,