Obstacle rewrite

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

@@ -61,7 +61,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

@@ -11,19 +11,34 @@ class Obstacle {
 public:
     Obstacle() = default;
 
-    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();
+    Obstacle(std::shared_ptr<rj_geometry::Shape> obs, std::shared_ptr<rj_geometry::Shape> pad)
+        : obstacle(obs), padding(pad) {}
+
+    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 padding_hit(rj_geometry::Point pt) { return padding->hit(pt); }
+
+    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);
+    }
+
+    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

src/rj_planning/include/rj_planning/plan_request.hpp

@@ -5,7 +5,6 @@
 #include <utility>
 
 #include <rj_common/context.hpp>
-#include <rj_common/planning/dynamic_obstacle.hpp>
 #include <rj_common/planning/instant.hpp>
 #include <rj_common/planning/motion_command.hpp>
 #include <rj_common/planning/motion_constraints.hpp>
@@ -29,7 +28,7 @@ namespace planning {
  */
 struct PlanRequest {
     PlanRequest(RobotInstant start, MotionCommand command,  // NOLINT
-                RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles,
+                RobotConstraints constraints, rj_geometry::ShapeSet global_obstacles,
                 rj_geometry::ShapeSet virtual_obstacles,
                 std::shared_ptr<TrajectoryCollection> planned_trajectories, unsigned shell_id,
                 const WorldState* world_state, PlayState play_state,
@@ -41,7 +40,7 @@ struct PlanRequest {
         : start(start),
           motion_command(command),  // NOLINT
           constraints(constraints),
-          field_obstacles(std::move(field_obstacles)),
+          global_obstacles(std::move(global_obstacles)),
           virtual_obstacles(std::move(virtual_obstacles)),
           planned_trajectories(planned_trajectories),
           shell_id(shell_id),
@@ -72,9 +71,9 @@ struct PlanRequest {
     RobotConstraints constraints;
 
     /**
-     * The list of field obstacles.
+     * The list of global obstacles.
      */
-    rj_geometry::ShapeSet field_obstacles;
+    rj_geometry::ShapeSet global_obstacles;
 
     /**
      * The list of "virtual" obstacles, generated by gameplay representing soft
@@ -174,20 +173,13 @@ void fill_robot_obstacle(const RobotState& robot, rj_geometry::Point& obs_center
  * Fill the obstacle fields.
  *
  * @param in the plan request.
- * @param out_static an (empty) vector of static obstacles to be populated.
+ * @param out_obstacles an (empty) vector of obstacles to be populated.
  *  This will be filled with field obstacles, local (virtual) obstacles,
- *  opponent robots, and our robots that have not yet been planned.
- * @param out_dynamic an (empty) vector of dynamic obstacles to be populated.
- *  This will be filled with trajectories for our robots that have already been
- *  planned.
+ *  opponent robots, and our robots.
  * @param avoid_ball whether to avoid the ball. If this is true, out_ball_trajectory
  *  should point to a valid trajectory.
- * @param ball_trajectory temporary storage for the ball trajectory. This must
- *  outlive the usage of out_dynamic. If avoid_ball == false, this should be
- *  nullptr.
  */
-void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static,
-                    std::vector<DynamicObstacle>* out_dynamic, bool avoid_ball,
-                    Trajectory* out_ball_trajectory = nullptr);
+void fill_obstacles(const PlanRequest& in, std::vector<std::shared_ptr<Obstacle>>& out_obstacles,
+                    bool avoid_ball);
 
 }  // namespace planning

src/rj_planning/include/rj_planning/planners/collect_path_planner.hpp

@@ -51,28 +51,21 @@ class CollectPathPlanner : public PathPlanner {
     void process_state_transition(const PlanRequest& request, BallState ball,
                                   RobotInstant* start_instant);
 
-    Trajectory coarse_approach(
-        const PlanRequest& plan_request, RobotInstant start,
-        const rj_geometry::ShapeSet& static_obstacles,
-        const std::vector<DynamicObstacle>& dynamic_obstacles);
+    Trajectory coarse_approach(const PlanRequest& plan_request, RobotInstant start,
+                               const rj_geometry::ShapeSet& static_obstacles);
 
     Trajectory intercept(const PlanRequest& plan_request, RobotInstant start_instant,
-                         const rj_geometry::ShapeSet& static_obstacles,
-                         const std::vector<DynamicObstacle>& dynamic_obstacles);
+                         const rj_geometry::ShapeSet& static_obstacles);
 
     // Dampen doesn't need to take obstacles into account.
     Trajectory dampen(const PlanRequest& plan_request, RobotInstant start_instant,
-                      const rj_geometry::ShapeSet& static_obstacles,
-                      const std::vector<DynamicObstacle>& dynamic_obstacles);
+                      const rj_geometry::ShapeSet& static_obstacles);
 
-    Trajectory fine_approach(
-        const PlanRequest& plan_request, RobotInstant start_instant,
-        const rj_geometry::ShapeSet& static_obstacles,
-        const std::vector<DynamicObstacle>& dynamic_obstacles);
+    Trajectory fine_approach(const PlanRequest& plan_request, RobotInstant start_instant,
+                             const rj_geometry::ShapeSet& static_obstacles);
 
     Trajectory invalid(const PlanRequest& plan_request,
-                       const rj_geometry::ShapeSet& static_obstacles,
-                       const std::vector<DynamicObstacle>& dynamic_obstacles);
+                       const rj_geometry::ShapeSet& static_obstacles);
 
     Trajectory previous_;
 

src/rj_planning/include/rj_planning/planners/settle_path_planner.hpp

@@ -78,18 +78,15 @@ class SettlePathPlanner : public PathPlanner {
     Trajectory intercept(const PlanRequest& plan_request,
                          RobotInstant start_instant,
                          const rj_geometry::ShapeSet& static_obstacles,
-                         const std::vector<DynamicObstacle>& dynamic_obstacles,
                          rj_geometry::Point delta_pos, rj_geometry::Point face_pos);
 
     // Dampen doesn't need to take obstacles into account.
     Trajectory dampen(const PlanRequest& plan_request, RobotInstant start_instant,
                       const rj_geometry::ShapeSet& static_obstacles,
-                      const std::vector<DynamicObstacle>& dynamic_obstacles,
                       rj_geometry::Point delta_pos, rj_geometry::Point face_pos);
 
     Trajectory invalid(const PlanRequest& plan_request,
-                       const rj_geometry::ShapeSet& static_obstacles,
-                       const std::vector<DynamicObstacle>& dynamic_obstacles);
+                       const rj_geometry::ShapeSet& static_obstacles);
 
     std::optional<rj_geometry::Point> target_bounce_direction_;
 

src/rj_planning/include/rj_planning/primitives/create_path.hpp

@@ -22,7 +22,6 @@ Trajectory rrt(const LinearMotionInstant& start,
                const LinearMotionInstant& goal,
                const MotionConstraints& motion_constraints, RJ::Time start_time,
                const rj_geometry::ShapeSet& static_obstacles,
-               const std::vector<DynamicObstacle>& dynamic_obstacles = {},
                const std::vector<rj_geometry::Point>& bias_waypoints = {});
 
 /**
@@ -36,7 +35,6 @@ Trajectory simple(
 Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
                         const MotionConstraints& motion_constraints, RJ::Time start_time,
                         const rj_geometry::ShapeSet& static_obstacles,
-                        const std::vector<DynamicObstacle>& dynamic_obstacles,
                         const FieldDimensions* field_dimensions, unsigned int robot_id);
 
 std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,

src/rj_planning/include/rj_planning/primitives/replanner.hpp

@@ -35,7 +35,6 @@ class Replanner {
         RobotInstant start;
         LinearMotionInstant goal;
         const rj_geometry::ShapeSet& static_obstacles;
-        const std::vector<DynamicObstacle>& dynamic_obstacles;
         const FieldDimensions* field_dimensions;
         RobotConstraints constraints;
         const AngleFunction& angle_function;

src/rj_planning/include/rj_planning/stadium_obstacle.hpp

@@ -3,9 +3,24 @@
 #include <rj_planning/obstacle.hpp>
 
 namespace planning {
-class StadiumObstacle : Obstacle {
+
+class StadiumObstacle : public Obstacle {
 public:
-    StadiumObstacle(rj_geometry::Point pos, rj_geometry::Point vel);
+    /**
+     * Creates a new obstacle with stadium shape padding.
+     * Intended for use with obstacles in motion.
+     * @param pos rj_geometry::Point representing the obstacle's position.
+     * @param vel rj_geometry::Point representing the obstacle's linear velocity.
+     */
+    StadiumObstacle(rj_geometry::Point pos, rj_geometry::Point vel) {
+        obstacle = std::make_shared<rj_geometry::Circle>(rj_geometry::Circle(pos, kRobotRadius));
+        padding = std::make_shared<rj_geometry::StadiumShape>(rj_geometry::StadiumShape(
+            pos, pos + vel * scaling, 1.5 * kRobotRadius + (vel.mag() * width_scaling)));
+    }
+
+private:
+    static constexpr float scaling = 0.5f;
+    static constexpr float width_scaling = 0.1f;
 };
 
 }  // namespace planning

src/rj_planning/include/rj_planning/static_obstacle.hpp

@@ -2,10 +2,20 @@
 #include <rj_planning/obstacle.hpp>
 
 namespace planning {
-class StaticObstacle : Obstacle {
+
+class StaticObstacle : public Obstacle {
 public:
+    /**
+     * Creates a new Obstacle with the desired shapes for the obstacle and padding.
+     * Note that if using compound shapes like StadiumShape, you should draw the shapes
+     * representing the obstacle using those shapes' draw() method, not using draw_shapes()
+     * on the Obstacle's shapeset.
+     * @param obstacle rj_geometry::Shape representing the obstacle; can be any Shape.
+     * @param padding rj_geometry::Shape representing the padding; can be any Shape.
+     */
     StaticObstacle(std::shared_ptr<rj_geometry::Shape> obstacle,
-                   std::shared_ptr<rj_geometry::Shape> padding);
+                   std::shared_ptr<rj_geometry::Shape> padding)
+        : Obstacle(obstacle, padding) {}
 };
 
 }  // namespace planning

src/rj_planning/include/rj_planning/trajectory_utils.hpp

@@ -25,21 +25,4 @@ bool trajectory_hits_static(const Trajectory& trajectory,
                           const rj_geometry::ShapeSet& obstacles,
                           RJ::Time start_time, RJ::Time* hit_time);
 
-/**
- * @brief Whether the given trajectory intersects any of the dynamic obstacles
- *  at any point along its path after a specified starting time.
- *
- * @param trajectory       The trajectory to check.
- * @param obstacles        A set of obstacles to check against.
- * @param start_time       A start time to the entire check.
- * @param out_hit_obstacle A bounding circle for the obstacle that was hit.
- * @param out_hit_time     The time of the collision (output parameter).
- * @return                 Whether or not there is a collision.
- */
-bool trajectory_hits_dynamic(const Trajectory& trajectory,
-                           const std::vector<DynamicObstacle>& obstacles,
-                           RJ::Time start_time,
-                           rj_geometry::Circle* out_hit_obstacle,
-                           RJ::Time* out_hit_time);
-
 }  // namespace planning

src/rj_planning/src/global_state.cpp

@@ -11,7 +11,7 @@ GlobalState::GlobalState(rclcpp::Node* node) {
                 last_play_state_ = rj_convert::convert_from_ros(*state);
                 have_play_state_ = true;
             }
-            set_static_obstacles();
+            set_field_obstacles();
         });
     game_settings_sub_ = node->create_subscription<rj_msgs::msg::GameSettings>(
         config_server::topics::kGameSettingsTopic, rclcpp::QoS(1),
@@ -45,7 +45,7 @@ GlobalState::GlobalState(rclcpp::Node* node) {
                 last_field_dimensions_ = rj_convert::convert_from_ros(*msg);
                 have_field_dimensions_ = true;
             }
-            set_static_obstacles();
+            set_field_obstacles();
         });
 }
 
@@ -115,7 +115,7 @@ rj_geometry::ShapeSet GlobalState::create_defense_area_obstacles() {
     return def_area_obstacles;
 }
 
-void GlobalState::set_static_obstacles() {
+void GlobalState::set_field_obstacles() {
     std::scoped_lock lock{last_field_dimensions_mutex_, last_play_state_mutex_,
                           last_def_area_obstacles_mutex_};