Obstacle rewrite

install/setup.bash

@@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null
 _colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
 
 unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_bash_source_script
+unset _colcon_prefix_chain_bash_source_script

install/setup.zsh

@@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && p
 _colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
 
 unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_zsh_source_script
+unset _colcon_prefix_chain_zsh_source_script

src/rj_common/include/rj_common/context.hpp

@@ -87,7 +87,6 @@ struct Context {
     /** \brief Whether at least one joystick is connected */
     bool joystick_valid = false;
 
-    rj_geometry::ShapeSet global_obstacles;
     rj_geometry::ShapeSet def_area_obstacles;
 
     PlayState play_state = PlayState::halt();

src/rj_constants/include/rj_constants/topic_names.hpp

@@ -59,7 +59,6 @@ constexpr auto kDebugTextTopic{"gameplay/debug_text"};
 
 namespace planning::topics {
 
-constexpr auto kGlobalObstaclesTopic{"planning/global_obstacles"};
 constexpr auto kDefAreaObstaclesTopic{"planning/def_area_obstacles"};
 
 static inline std::string trajectory_topic(int robot_id) {

src/rj_geometry/include/rj_geometry/stadium_shape.hpp

@@ -4,89 +4,46 @@
 #include <set>
 #include <vector>
 
+#include "composite_shape.hpp"
 #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.
+ * A rj_geometry::StadiumShape is a CompositeShape made up of 2 circles and a polygon.
+ * It represents the shape of a track from track and field.
  */
-class StadiumShape : public Shape {
+class StadiumShape : public CompositeShape {
 public:
     ~StadiumShape() = default;
 
     StadiumShape() = default;
 
-    StadiumShape(Point c1, Point c2, float r) { init(c1, c2, r); }
+    StadiumShape(Point c1, Point c2, float 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);
-        }
+    StadiumShape(const StadiumShape& other) : CompositeShape(other) {
         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_) {
+        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_;
 };
 

src/rj_geometry/src/stadium_shape.cpp

@@ -4,55 +4,32 @@ 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)};
+StadiumShape::StadiumShape(Point c1, Point c2, float r) {
+    // Create the two circular end caps
+    auto c1_obs_ptr = std::make_shared<Circle>(c1, r);
+    auto c2_obs_ptr = std::make_shared<Circle>(c2, r);
 
-    rj_geometry::Segment vect{c1, c2};
-    rj_geometry::Point leftToRight{c2.x() - c1.x(), c2.y() - c1.y()};
-    rj_geometry::Point leftToRightN = leftToRight.norm().perp_ccw();
+    // Create the rectangular middle section connecting the circles
+    Point leftToRight{c2.x() - c1.x(), c2.y() - c1.y()};
+    Point leftToRightN = leftToRight.norm().perp_ccw();
 
-    rj_geometry::Point leftTop = c1 + (leftToRightN * r);
-    rj_geometry::Point leftBottom = c1 - (leftToRightN * r);
-    rj_geometry::Point rightTop = c2 + (leftToRightN * r);
-    rj_geometry::Point rightBottom = c2 - (leftToRightN * r);
+    Point leftTop = c1 + (leftToRightN * r);
+    Point leftBottom = c1 - (leftToRightN * r);
+    Point rightTop = c2 + (leftToRightN * r);
+    Point rightBottom = c2 - (leftToRightN * r);
 
-    std::vector<Point> verts = {leftTop, leftBottom, rightTop, rightBottom};
+    std::vector<Point> verts = {leftTop, rightTop, rightBottom, leftBottom};
+    auto rect_obs_ptr = std::make_shared<Polygon>(verts);
 
-    rj_geometry::Polygon rect_obs{verts};
-
-    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);
+    // Use CompositeShape's add() method
+    add(c1_obs_ptr);
+    add(rect_obs_ptr);
+    add(c2_obs_ptr);
 
+    // Also store in drawshapes_ for backward compatibility
     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

src/rj_planning/CMakeLists.txt

@@ -55,9 +55,6 @@ set(RJ_PLANNING_SRC
   src/planner_node.cpp
   src/trajectory_collection.cpp
   src/trajectory_utils.cpp
-  src/obstacle.cpp
-  src/static_obstacle.cpp
-  src/stadium_obstacle.cpp
 
   # Planners
   src/planners/collect_path_planner.cpp

src/rj_planning/include/rj_planning/global_state.hpp

@@ -30,7 +30,6 @@ class GlobalState {
     [[nodiscard]] PlayState play_state() const;
     [[nodiscard]] GameSettings game_settings() const;
     [[nodiscard]] int goalie_id() const;
-    [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const;
     [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const;
     [[nodiscard]] const WorldState* world_state() const;
     [[nodiscard]] const FieldDimensions* field_dimensions() const;
@@ -39,7 +38,6 @@ class GlobalState {
     rclcpp::Subscription<rj_msgs::msg::PlayState>::SharedPtr play_state_sub_;
     rclcpp::Subscription<rj_msgs::msg::GameSettings>::SharedPtr game_settings_sub_;
     rclcpp::Subscription<rj_msgs::msg::Goalie>::SharedPtr goalie_sub_;
-    rclcpp::Subscription<rj_geometry_msgs::msg::ShapeSet>::SharedPtr global_obstacles_sub_;
     rclcpp::Subscription<rj_msgs::msg::WorldState>::SharedPtr world_state_sub_;
     rclcpp::Subscription<rj_msgs::msg::FieldDimensions>::SharedPtr field_dimensions_sub_;
 
@@ -50,8 +48,6 @@ class GlobalState {
     mutable std::mutex last_game_settings_mutex_{};
     int last_goalie_id_{};
     mutable std::mutex last_goalie_id_mutex_{};
-    rj_geometry::ShapeSet last_global_obstacles_{};
-    mutable std::mutex last_global_obstacles_mutex_{};
     rj_geometry::ShapeSet last_def_area_obstacles_{};
     mutable std::mutex last_def_area_obstacles_mutex_{};
     WorldState last_world_state_{};
@@ -61,7 +57,7 @@ class GlobalState {
     mutable std::mutex last_field_dimensions_mutex_{};
 
     rj_geometry::ShapeSet create_defense_area_obstacles();
-    void set_static_obstacles();
+    void set_field_obstacles();
 };
 
 }  // namespace planning

src/rj_planning/include/rj_planning/obstacle.hpp

@@ -1,29 +1,53 @@
+#pragma once
 #include <rj_common/context.hpp>
 #include <rj_common/planning/robot_constraints.hpp>
 #include <rj_geometry/circle.hpp>
 #include <rj_geometry/point.hpp>
+#include <rj_geometry/segment.hpp>
 #include <rj_geometry/shape.hpp>
 #include <rj_geometry/stadium_shape.hpp>
 
 namespace planning {
 
 class Obstacle {
 public:
-    Obstacle() = default;
+    Obstacle(std::shared_ptr<rj_geometry::Shape> obs, std::shared_ptr<rj_geometry::Shape> pad)
+        : obstacle(obs), padding(pad) {}
 
-    virtual bool obstacle_hit(rj_geometry::Point pt);
-    virtual bool padding_hit(rj_geometry::Point pt);
-    virtual bool obstacle_near(rj_geometry::Point pt, float thresh);
-    virtual bool padding_near(rj_geometry::Point pt, float thresh);
-    virtual shared_ptr<rj_geometry::ShapeSet> get_shapes();
-    virtual std::shared_ptr<rj_geometry::Shape> get_obstacle();
-    virtual std::shared_ptr<rj_geometry::Shape> get_padding();
+    virtual ~Obstacle() = default;
+    Obstacle(const Obstacle& other) = default;
+    Obstacle(Obstacle&& other) = default;
+    Obstacle& operator=(const Obstacle& other) = default;
+    Obstacle& operator=(Obstacle&& other) = default;
+
+    virtual bool obstacle_hit(rj_geometry::Point pt) { return obstacle->hit(pt); }
+
+    virtual bool obstacle_hit(const rj_geometry::Segment& seg) { return obstacle->hit(seg); }
+
+    virtual bool padding_hit(rj_geometry::Point pt) { return padding->hit(pt); }
+
+    virtual bool padding_hit(const rj_geometry::Segment& seg) { return padding->hit(seg); }
+
+    virtual bool obstacle_near(rj_geometry::Point pt, float thresh) {
+        return obstacle->near_point(pt, thresh);
+    }
+
+    virtual bool padding_near(rj_geometry::Point pt, float thresh) {
+        return padding->near_point(pt, thresh);
+    }
+
+    // Convenience methods: hit() delegates to padding_hit() for compatibility
+    virtual bool hit(rj_geometry::Point pt) { return padding_hit(pt); }
+
+    virtual bool hit(const rj_geometry::Segment& seg) { return padding_hit(seg); }
+
+    virtual std::shared_ptr<rj_geometry::Shape> get_obstacle() { return obstacle; }
+
+    virtual std::shared_ptr<rj_geometry::Shape> get_padding() { return padding; }
 
 protected:
     std::shared_ptr<rj_geometry::Shape> obstacle;
     std::shared_ptr<rj_geometry::Shape> padding;
-    std::shared_ptr<rj_geometry::Point> velocity;
-    rj_geometry::ShapeSet shapes;
 };
 
 }  // namespace planning