diff --git a/src/rj_planning/CMakeLists.txt b/src/rj_planning/CMakeLists.txt index 2796055865..1df3f64179 100644 --- a/src/rj_planning/CMakeLists.txt +++ b/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 diff --git a/src/rj_planning/include/rj_planning/global_state.hpp b/src/rj_planning/include/rj_planning/global_state.hpp index 2311586313..e7ffbc8ab8 100644 --- a/src/rj_planning/include/rj_planning/global_state.hpp +++ b/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 diff --git a/src/rj_planning/include/rj_planning/obstacle.hpp b/src/rj_planning/include/rj_planning/obstacle.hpp index 52b31eecc7..de6d65f0f2 100644 --- a/src/rj_planning/include/rj_planning/obstacle.hpp +++ b/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 get_shapes(); - virtual std::shared_ptr get_obstacle(); - virtual std::shared_ptr get_padding(); + Obstacle(std::shared_ptr obs, std::shared_ptr 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 get_obstacle() { return obstacle; } + + virtual std::shared_ptr get_padding() { return padding; } protected: std::shared_ptr obstacle; std::shared_ptr padding; - std::shared_ptr velocity; - rj_geometry::ShapeSet shapes; }; } // namespace planning \ No newline at end of file diff --git a/src/rj_planning/include/rj_planning/plan_request.hpp b/src/rj_planning/include/rj_planning/plan_request.hpp index ef905c45d7..d0e059a889 100644 --- a/src/rj_planning/include/rj_planning/plan_request.hpp +++ b/src/rj_planning/include/rj_planning/plan_request.hpp @@ -5,7 +5,6 @@ #include #include -#include #include #include #include @@ -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 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* out_dynamic, bool avoid_ball, - Trajectory* out_ball_trajectory = nullptr); +void fill_obstacles(const PlanRequest& in, std::vector>& out_obstacles, + bool avoid_ball); } // namespace planning diff --git a/src/rj_planning/include/rj_planning/planners/collect_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/collect_path_planner.hpp index 5d4e6d9a89..c574acb961 100644 --- a/src/rj_planning/include/rj_planning/planners/collect_path_planner.hpp +++ b/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& 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& 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& 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& 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& dynamic_obstacles); + const rj_geometry::ShapeSet& static_obstacles); Trajectory previous_; diff --git a/src/rj_planning/include/rj_planning/planners/settle_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/settle_path_planner.hpp index 231e45393b..c21d296154 100644 --- a/src/rj_planning/include/rj_planning/planners/settle_path_planner.hpp +++ b/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& 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& 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& dynamic_obstacles); + const rj_geometry::ShapeSet& static_obstacles); std::optional target_bounce_direction_; diff --git a/src/rj_planning/include/rj_planning/primitives/create_path.hpp b/src/rj_planning/include/rj_planning/primitives/create_path.hpp index db6f830c8d..33066bf3d2 100644 --- a/src/rj_planning/include/rj_planning/primitives/create_path.hpp +++ b/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& dynamic_obstacles = {}, const std::vector& 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& dynamic_obstacles, const FieldDimensions* field_dimensions, unsigned int robot_id); std::vector get_intermediates(const LinearMotionInstant& start, diff --git a/src/rj_planning/include/rj_planning/primitives/replanner.hpp b/src/rj_planning/include/rj_planning/primitives/replanner.hpp index 8f3d1df6a0..356f70aada 100644 --- a/src/rj_planning/include/rj_planning/primitives/replanner.hpp +++ b/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& dynamic_obstacles; const FieldDimensions* field_dimensions; RobotConstraints constraints; const AngleFunction& angle_function; diff --git a/src/rj_planning/include/rj_planning/stadium_obstacle.hpp b/src/rj_planning/include/rj_planning/stadium_obstacle.hpp index 3f23f3ab63..916baa3cf8 100644 --- a/src/rj_planning/include/rj_planning/stadium_obstacle.hpp +++ b/src/rj_planning/include/rj_planning/stadium_obstacle.hpp @@ -3,9 +3,24 @@ #include 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(pos, kRobotRadius)); + padding = std::make_shared(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 \ No newline at end of file diff --git a/src/rj_planning/include/rj_planning/static_obstacle.hpp b/src/rj_planning/include/rj_planning/static_obstacle.hpp index 59a239dde6..003353e4d3 100644 --- a/src/rj_planning/include/rj_planning/static_obstacle.hpp +++ b/src/rj_planning/include/rj_planning/static_obstacle.hpp @@ -2,10 +2,20 @@ #include 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 obstacle, - std::shared_ptr padding); + std::shared_ptr padding) + : Obstacle(obstacle, padding) {} }; } // namespace planning diff --git a/src/rj_planning/include/rj_planning/trajectory_utils.hpp b/src/rj_planning/include/rj_planning/trajectory_utils.hpp index 98c9e7c3de..35750b6ad4 100644 --- a/src/rj_planning/include/rj_planning/trajectory_utils.hpp +++ b/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& obstacles, - RJ::Time start_time, - rj_geometry::Circle* out_hit_obstacle, - RJ::Time* out_hit_time); - } // namespace planning diff --git a/src/rj_planning/src/global_state.cpp b/src/rj_planning/src/global_state.cpp index 2ce424b6ee..c0fbdee425 100644 --- a/src/rj_planning/src/global_state.cpp +++ b/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( 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_}; diff --git a/src/rj_planning/src/obstacle.cpp b/src/rj_planning/src/obstacle.cpp deleted file mode 100644 index 67380ff6f3..0000000000 --- a/src/rj_planning/src/obstacle.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include - -namespace planning { - -bool Obstacle::obstacle_hit(rj_geometry::Point pt) { return obstacle->hit(pt); } - -bool Obstacle::padding_hit(rj_geometry::Point pt) { return padding->hit(pt); } - -bool Obstacle::padding_near(rj_geometry::Point pt, float thresh) { - return padding->near_point(pt, thresh); -} - -bool Obstacle::obstacle_near(rj_geometry::Point pt, float thresh) { - return obstacle->near_point(pt, thresh); -} - -std::shared_ptr Obstacle::get_obstacle() { return obstacle; } - -std::shared_ptr Obstacle::get_padding() { return padding; } - -std::shared_ptr Obstacle::get_shapes() { - return std::make_shared(shapes); -} -} // namespace planning \ No newline at end of file diff --git a/src/rj_planning/src/plan_request.cpp b/src/rj_planning/src/plan_request.cpp index 698f2f3070..86074fe1ec 100644 --- a/src/rj_planning/src/plan_request.cpp +++ b/src/rj_planning/src/plan_request.cpp @@ -20,12 +20,14 @@ rj_geometry::Circle make_robot_obstacle(const RobotState& robot) { return make_inflated_static_obs(robot.pose.position(), robot.velocity.linear(), kRobotRadius); } -void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, - std::vector* out_dynamic, bool avoid_ball, - Trajectory* out_ball_trajectory) { - out_static->clear(); - out_static->add(in.field_obstacles); - out_static->add(in.virtual_obstacles); +void fill_obstacles(const PlanRequest& in, std::vector>& out_obstacles, + bool avoid_ball) { + out_obstacles.clear(); + + // TODO: Convert in.global_obstacles and in.virtual_obstacles to Obstacle objects + // Assume these obstacles have identical padding to shape + // out_obstacles.add(in.global_obstacles); + // out_obstacles.add(in.virtual_obstacles); // Add their robots as static obstacles (inflated based on velocity). // See calc_static_robot_obs() docstring for more info. @@ -33,16 +35,13 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, const RobotState& their_robot = in.world_state->their_robots.at(shell); if (their_robot.visible) { - out_static->add( - std::make_shared(make_robot_obstacle(their_robot))); + // TODO: Create Obstacle object instead of adding to ShapeSet + // out_obstacles.add( + // std::make_shared(make_robot_obstacle(their_robot))); } } - // Add our robots, either static or dynamic depending on whether they have - // already been planned. In both cases, radius is based on velocity like - // above for opp robots. - // TODO: reenable dynamic obstacles for our robots (currently - // TrajectoryCollection is never filled at planner level) + // Add our robots as static obstacles (inflated based on velocity). for (size_t shell = 0; shell < kNumShells; shell++) { const auto& our_robot = in.world_state->our_robots.at(shell); if (!our_robot.visible || shell == in.shell_id) { @@ -50,7 +49,8 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, } // Static obstacle - out_static->add(std::make_shared(make_robot_obstacle(our_robot))); + // TODO: Create Obstacle object instead of adding to ShapeSet + // out_obstacles.add(std::make_shared(make_robot_obstacle(our_robot))); } // Adding ball as a static obstacle (because dynamic obstacles are not working) @@ -67,7 +67,8 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, in.debug_drawer->draw_circle(ball_obs, draw_color); } - out_static->add(std::make_shared(std::move(ball_obs))); + // TODO: Create Obstacle object instead of adding to ShapeSet + // out_obstacles.add(std::make_shared(std::move(ball_obs))); auto maybe_bp_point = in.play_state.ball_placement_point(); if (maybe_bp_point.has_value() && in.play_state.is_their_restart()) { @@ -78,7 +79,8 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, std::shared_ptr track_obs_ptr = std::make_shared(stadium); - out_static->add(track_obs_ptr); + // TODO: Create Obstacle object instead of adding to ShapeSet + // out_obstacles.add(track_obs_ptr); if (in.debug_drawer != nullptr) { QColor draw_color = Qt::red; diff --git a/src/rj_planning/src/planner_for_robot.cpp b/src/rj_planning/src/planner_for_robot.cpp index 2613255bfc..28a84bfc41 100644 --- a/src/rj_planning/src/planner_for_robot.cpp +++ b/src/rj_planning/src/planner_for_robot.cpp @@ -159,9 +159,6 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { const auto& robot = world_state->our_robots.at(robot_id_); const auto start = RobotInstant{robot.pose, robot.velocity, robot.timestamp}; - const auto global_obstacles = global_state_.global_obstacles(); - rj_geometry::ShapeSet real_obstacles = global_obstacles; - const auto def_area_obstacles = global_state_.def_area_obstacles(); rj_geometry::ShapeSet virtual_obstacles = intent.local_obstacles; const bool is_goalie = goalie_id == robot_id_; @@ -192,7 +189,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { return PlanRequest{start, motion_command, constraints, - std::move(real_obstacles), + global_state_.global_obstacles(), std::move(virtual_obstacles), robot_trajectories_, static_cast(robot_id_), diff --git a/src/rj_planning/src/planners/collect_path_planner.cpp b/src/rj_planning/src/planners/collect_path_planner.cpp index b2a3c42bc3..51b33e183c 100644 --- a/src/rj_planning/src/planners/collect_path_planner.cpp +++ b/src/rj_planning/src/planners/collect_path_planner.cpp @@ -93,9 +93,8 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { process_state_transition(plan_request, ball, &start_instant); // List of obstacles - ShapeSet static_obstacles; - std::vector dynamic_obstacles; - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false); + std::vector> static_obstacles; + fill_obstacles(plan_request, static_obstacles, false); // Return an empty trajectory if the ball is hitting static obstacles // or it is in the goalie area. @@ -107,26 +106,24 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { switch (current_state_) { // Moves from the current location to the slow point of approach case COARSE_APPROACH: - previous_ = - coarse_approach(plan_request, start_instant, static_obstacles, dynamic_obstacles); + previous_ = coarse_approach(plan_request, start_instant, static_obstacles); break; // Moves from the slow point of approach to just before point of contact case FINE_APPROACH: - previous_ = - fine_approach(plan_request, start_instant, static_obstacles, dynamic_obstacles); + previous_ = fine_approach(plan_request, start_instant, static_obstacles); break; // Intercept a moving ball case INTERCEPT: { - previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles); + previous_ = intercept(plan_request, start_instant, static_obstacles); break; } // Dampen a moving ball case DAMPEN: { - previous_ = dampen(plan_request, start_instant, static_obstacles, dynamic_obstacles); + previous_ = dampen(plan_request, start_instant, static_obstacles); break; } default: - previous_ = invalid(plan_request, static_obstacles, dynamic_obstacles); + previous_ = invalid(plan_request, static_obstacles); break; } @@ -196,10 +193,8 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba is_ball_sense_ = request.ball_sense && current_state_ == FINE_APPROACH; } -Trajectory CollectPathPlanner::coarse_approach( - const PlanRequest& plan_request, RobotInstant start, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { +Trajectory CollectPathPlanner::coarse_approach(const PlanRequest& plan_request, RobotInstant start, + const rj_geometry::ShapeSet& static_obstacles) { BallState ball = plan_request.world_state->ball; // There are two paths that get combined together @@ -236,7 +231,6 @@ Trajectory CollectPathPlanner::coarse_approach( Replanner::PlanParams params{start, target_slow, static_obstacles, - dynamic_obstacles, plan_request.field_dimensions, plan_request.constraints, AngleFns::face_point(ball.position), @@ -259,8 +253,7 @@ Trajectory CollectPathPlanner::coarse_approach( Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, RobotInstant start_instant, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { + const rj_geometry::ShapeSet& static_obstacles) { const double max_ball_angle_change_for_path_reset = settle::PARAM_max_ball_angle_for_reset * M_PI / 180.0f; @@ -318,7 +311,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, // test location Trajectory path = CreatePath::intermediate( start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot, - start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + start_instant.stamp, static_obstacles, plan_request.field_dimensions, plan_request.shell_id); // Calculate the @@ -428,7 +421,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, Trajectory shortcut = CreatePath::intermediate( start_instant.linear_motion(), target, plan_request.constraints.mot, - start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + start_instant.stamp, static_obstacles, plan_request.field_dimensions, plan_request.shell_id); if (!shortcut.empty()) { @@ -466,7 +459,6 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, Replanner::PlanParams params{start_instant, target_robot_intersection, static_obstacles, - dynamic_obstacles, plan_request.field_dimensions, plan_request.constraints, AngleFns::face_point(face_pos), @@ -487,8 +479,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, } Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInstant start_instant, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { + const rj_geometry::ShapeSet& static_obstacles) { // Only run once if we can // Intercept ends with a % ball velocity in the direction of the ball @@ -576,13 +567,13 @@ Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInst if (previous_.empty()) { dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion, plan_request.constraints.mot, start_instant.stamp, - static_obstacles, dynamic_obstacles, - plan_request.field_dimensions, plan_request.shell_id); + static_obstacles, plan_request.field_dimensions, + plan_request.shell_id); } else { - dampen_end = CreatePath::intermediate( - previous_.last().linear_motion(), final_stopping_motion, plan_request.constraints.mot, - previous_.last().stamp, static_obstacles, dynamic_obstacles, - plan_request.field_dimensions, plan_request.shell_id); + dampen_end = CreatePath::intermediate(previous_.last().linear_motion(), + final_stopping_motion, plan_request.constraints.mot, + previous_.last().stamp, static_obstacles, + plan_request.field_dimensions, plan_request.shell_id); } dampen_end.set_debug_text("Damping"); @@ -598,10 +589,9 @@ Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInst return dampen_end; } -Trajectory CollectPathPlanner::fine_approach( - const PlanRequest& plan_request, RobotInstant start_instant, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { +Trajectory CollectPathPlanner::fine_approach(const PlanRequest& plan_request, + RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles) { BallState ball = plan_request.world_state->ball; RobotConstraints robot_constraints_hit = plan_request.constraints; MotionConstraints& motion_constraints_hit = robot_constraints_hit.mot; @@ -637,7 +627,6 @@ Trajectory CollectPathPlanner::fine_approach( Replanner::PlanParams params{start_instant, target_hit, static_obstacles, - dynamic_obstacles, plan_request.field_dimensions, plan_request.constraints, AngleFns::face_point(ball.position), @@ -666,8 +655,7 @@ Trajectory CollectPathPlanner::fine_approach( } Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { + const rj_geometry::ShapeSet& static_obstacles) { current_state_ = COARSE_APPROACH; // Stop movement until next frame since it's the safest option @@ -677,7 +665,6 @@ Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request, Replanner::PlanParams params{plan_request.start, target, static_obstacles, - dynamic_obstacles, plan_request.field_dimensions, plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position), diff --git a/src/rj_planning/src/planners/escape_obstacles_path_planner.cpp b/src/rj_planning/src/planners/escape_obstacles_path_planner.cpp index a631d58709..d28bfc4440 100644 --- a/src/rj_planning/src/planners/escape_obstacles_path_planner.cpp +++ b/src/rj_planning/src/planners/escape_obstacles_path_planner.cpp @@ -7,8 +7,8 @@ Trajectory EscapeObstaclesPathPlanner::plan(const PlanRequest& plan_request) { const RobotInstant& start_instant = plan_request.start; const auto& motion_constraints = plan_request.constraints.mot; - rj_geometry::ShapeSet obstacles; - fill_obstacles(plan_request, &obstacles, nullptr, true, nullptr); + std::vector> obstacles; + fill_obstacles(plan_request, obstacles, true); if (!obstacles.hit(start_instant.position())) { // Keep moving, but slow down the current velocity. This allows us to @@ -34,7 +34,7 @@ Trajectory EscapeObstaclesPathPlanner::plan(const PlanRequest& plan_request) { path_obstacles.add(std::make_shared(ball)); auto result = CreatePath::intermediate(start_instant.linear_motion(), goal, motion_constraints, - start_instant.stamp, path_obstacles, {}, + start_instant.stamp, path_obstacles, plan_request.field_dimensions, plan_request.shell_id); plan_angles(&result, start_instant, AngleFns::tangent, plan_request.constraints.rot); result.set_debug_text("[ESCAPE " + std::to_string(plan_request.shell_id) + "]"); diff --git a/src/rj_planning/src/planners/goalie_idle_path_planner.cpp b/src/rj_planning/src/planners/goalie_idle_path_planner.cpp index 00bc04edcc..c2144bee15 100644 --- a/src/rj_planning/src/planners/goalie_idle_path_planner.cpp +++ b/src/rj_planning/src/planners/goalie_idle_path_planner.cpp @@ -7,12 +7,9 @@ Trajectory GoalieIdlePathPlanner::plan(const PlanRequest& plan_request) { // an easy way to convert from one PlanRequest to another // Collect obstacles - rj_geometry::ShapeSet static_obstacles; - std::vector dynamic_obstacles; - Trajectory ball_trajectory; + std::vector> static_obstacles; bool ignore_ball = true; - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, ignore_ball, - &ball_trajectory); + fill_obstacles(plan_request, static_obstacles, ignore_ball); // If we start inside of an obstacle, give up and let another planner take // care of it. @@ -30,7 +27,7 @@ Trajectory GoalieIdlePathPlanner::plan(const PlanRequest& plan_request) { // call Replanner to generate a Trajectory Trajectory trajectory = Replanner::create_plan( - Replanner::PlanParams{plan_request.start, target, static_obstacles, dynamic_obstacles, + Replanner::PlanParams{plan_request.start, target, static_obstacles, plan_request.field_dimensions, plan_request.constraints, angle_function, plan_request.shell_id, RJ::Seconds(3.0)}, std::move(previous_)); diff --git a/src/rj_planning/src/planners/line_pivot_path_planner.cpp b/src/rj_planning/src/planners/line_pivot_path_planner.cpp index 614e397f21..b4bcc23b5c 100644 --- a/src/rj_planning/src/planners/line_pivot_path_planner.cpp +++ b/src/rj_planning/src/planners/line_pivot_path_planner.cpp @@ -73,9 +73,8 @@ Trajectory LinePivotPathPlanner::pivot(const PlanRequest& request) { const auto& linear_constraints = request.constraints.mot; const auto& rotation_constraints = request.constraints.rot; - rj_geometry::ShapeSet static_obstacles; - std::vector dynamic_obstacles; - fill_obstacles(request, &static_obstacles, &dynamic_obstacles, false); + std::vector> static_obstacles; + fill_obstacles(request, static_obstacles, false); const MotionCommand& command = request.motion_command; diff --git a/src/rj_planning/src/planners/path_target_path_planner.cpp b/src/rj_planning/src/planners/path_target_path_planner.cpp index 77b2c2b5de..8b5273ffed 100644 --- a/src/rj_planning/src/planners/path_target_path_planner.cpp +++ b/src/rj_planning/src/planners/path_target_path_planner.cpp @@ -6,12 +6,9 @@ namespace planning { Trajectory PathTargetPathPlanner::plan(const PlanRequest& request) { // Collect obstacles - ShapeSet static_obstacles; - std::vector dynamic_obstacles; - Trajectory ball_trajectory; + std::vector> static_obstacles; const MotionCommand& command = request.motion_command; - fill_obstacles(request, &static_obstacles, &dynamic_obstacles, !command.ignore_ball, - &ball_trajectory); + fill_obstacles(request, static_obstacles, !command.ignore_ball); // If we start inside of an obstacle, give up and let another planner take // care of it. @@ -32,9 +29,8 @@ Trajectory PathTargetPathPlanner::plan(const PlanRequest& request) { // Call into the sub-object to actually execute the plan. Trajectory trajectory = Replanner::create_plan( Replanner::PlanParams{request.start, target_instant, static_obstacles, - std::vector{}, request.field_dimensions, - request.constraints, angle_function, request.shell_id, - RJ::Seconds(3.0)}, + request.field_dimensions, request.constraints, angle_function, + request.shell_id, RJ::Seconds(3.0)}, std::move(previous_)); previous_ = trajectory; diff --git a/src/rj_planning/src/planners/pivot_path_planner.cpp b/src/rj_planning/src/planners/pivot_path_planner.cpp index 892d9db398..1550523cd6 100644 --- a/src/rj_planning/src/planners/pivot_path_planner.cpp +++ b/src/rj_planning/src/planners/pivot_path_planner.cpp @@ -8,9 +8,8 @@ Trajectory PivotPathPlanner::plan(const PlanRequest& request) { const auto& linear_constraints = request.constraints.mot; const auto& rotation_constraints = request.constraints.rot; - rj_geometry::ShapeSet static_obstacles; - std::vector dynamic_obstacles; - fill_obstacles(request, &static_obstacles, &dynamic_obstacles, false); + std::vector> static_obstacles; + fill_obstacles(request, static_obstacles, false); const MotionCommand& command = request.motion_command; diff --git a/src/rj_planning/src/planners/rotate_path_planner.cpp b/src/rj_planning/src/planners/rotate_path_planner.cpp index 956a44b4a2..3a63642446 100644 --- a/src/rj_planning/src/planners/rotate_path_planner.cpp +++ b/src/rj_planning/src/planners/rotate_path_planner.cpp @@ -39,9 +39,8 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { const auto& linear_constraints = request.constraints.mot; const auto& rotation_constraints = request.constraints.rot; - rj_geometry::ShapeSet static_obstacles; - std::vector dynamic_obstacles; - fill_obstacles(request, &static_obstacles, &dynamic_obstacles, false); + std::vector> static_obstacles; + fill_obstacles(request, static_obstacles, false); const MotionCommand& command = request.motion_command; diff --git a/src/rj_planning/src/planners/settle_path_planner.cpp b/src/rj_planning/src/planners/settle_path_planner.cpp index 87e1ab0d3a..cd43416c5f 100644 --- a/src/rj_planning/src/planners/settle_path_planner.cpp +++ b/src/rj_planning/src/planners/settle_path_planner.cpp @@ -28,11 +28,8 @@ Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { bool avoid_ball = true; // List of obstacles - ShapeSet static_obstacles; - std::vector dynamic_obstacles; - Trajectory ball_trajectory; - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, avoid_ball, - &ball_trajectory); + std::vector> static_obstacles; + fill_obstacles(plan_request, static_obstacles, avoid_ball); // Smooth out the ball velocity a little bit so we can get a better estimate // of intersect points @@ -73,15 +70,13 @@ Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { // Run state code switch (current_state_) { case SettlePathPlannerStates::Intercept: - result = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles, - delta_pos, face_pos); + result = intercept(plan_request, start_instant, static_obstacles, delta_pos, face_pos); break; case SettlePathPlannerStates::Dampen: - result = dampen(plan_request, start_instant, static_obstacles, dynamic_obstacles, - delta_pos, face_pos); + result = dampen(plan_request, start_instant, static_obstacles, delta_pos, face_pos); break; default: - result = invalid(plan_request, static_obstacles, dynamic_obstacles); + result = invalid(plan_request, static_obstacles); break; } @@ -162,7 +157,6 @@ void SettlePathPlanner::process_state_transition(BallState ball, RobotInstant* s Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles, rj_geometry::Point delta_pos, rj_geometry::Point face_pos) { BallState ball = plan_request.world_state->ball; @@ -211,7 +205,7 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn // test location Trajectory path = CreatePath::intermediate( start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot, - start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + start_instant.stamp, static_obstacles, plan_request.field_dimensions, plan_request.shell_id); // Calculate the @@ -323,7 +317,7 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn Trajectory shortcut = CreatePath::intermediate( start_instant.linear_motion(), target, plan_request.constraints.mot, - start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + start_instant.stamp, static_obstacles, plan_request.field_dimensions, plan_request.shell_id); if (!shortcut.empty()) { @@ -354,7 +348,6 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn Replanner::PlanParams params{start_instant, target_robot_intersection, static_obstacles, - dynamic_obstacles, plan_request.field_dimensions, plan_request.constraints, AngleFns::face_point(face_pos), @@ -376,7 +369,6 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles, rj_geometry::Point delta_pos, rj_geometry::Point face_pos) { // Only run once if we can @@ -462,13 +454,13 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta if (previous_.empty()) { dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion, plan_request.constraints.mot, start_instant.stamp, - static_obstacles, dynamic_obstacles, - plan_request.field_dimensions, plan_request.shell_id); + static_obstacles, plan_request.field_dimensions, + plan_request.shell_id); } else { - dampen_end = CreatePath::intermediate( - previous_.last().linear_motion(), final_stopping_motion, plan_request.constraints.mot, - previous_.last().stamp, static_obstacles, dynamic_obstacles, - plan_request.field_dimensions, plan_request.shell_id); + dampen_end = CreatePath::intermediate(previous_.last().linear_motion(), + final_stopping_motion, plan_request.constraints.mot, + previous_.last().stamp, static_obstacles, + plan_request.field_dimensions, plan_request.shell_id); } dampen_end.set_debug_text("Damping"); @@ -484,8 +476,7 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta } Trajectory SettlePathPlanner::invalid(const PlanRequest& plan_request, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { + const rj_geometry::ShapeSet& static_obstacles) { SPDLOG_WARN("Invalid state in settle planner. Restarting"); current_state_ = SettlePathPlannerStates::Intercept; @@ -496,7 +487,6 @@ Trajectory SettlePathPlanner::invalid(const PlanRequest& plan_request, Replanner::PlanParams params{plan_request.start, target, static_obstacles, - dynamic_obstacles, plan_request.field_dimensions, plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position), diff --git a/src/rj_planning/src/primitives/create_path.cpp b/src/rj_planning/src/primitives/create_path.cpp index d50ac223f9..cdb4891633 100644 --- a/src/rj_planning/src/primitives/create_path.cpp +++ b/src/rj_planning/src/primitives/create_path.cpp @@ -22,7 +22,6 @@ Trajectory simple(const LinearMotionInstant& start, const LinearMotionInstant& g Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal, const MotionConstraints& motion_constraints, RJ::Time start_time, const ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles, const std::vector& bias_waypoints) { // if already on goal, no need to move if (start.position.dist_to(goal.position) < 1e-6) { @@ -36,35 +35,17 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal // 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) && - !trajectory_hits_dynamic(straight_trajectory, dynamic_obstacles, start_time, nullptr, - nullptr))) { + !trajectory_hits_static(straight_trajectory, static_obstacles, start_time, nullptr)) { return straight_trajectory; } - ShapeSet obstacles = static_obstacles; - Trajectory path{{}}; - constexpr int kAttemptsToAvoidDynamics = 10; - for (int i = 0; i < kAttemptsToAvoidDynamics; i++) { - std::vector points = - generate_rrt(start.position, goal.position, obstacles, bias_waypoints); + std::vector points = + generate_rrt(start.position, goal.position, static_obstacles, bias_waypoints); - BezierPath post_bezier(points, start.velocity, goal.velocity, motion_constraints); + BezierPath post_bezier(points, start.velocity, goal.velocity, motion_constraints); - path = profile_velocity(post_bezier, start.velocity.mag(), goal.velocity.mag(), - motion_constraints, start_time); - - Circle hit_circle; - if (!trajectory_hits_dynamic(path, dynamic_obstacles, path.begin_time(), &hit_circle, - nullptr)) { - break; - } - - // Inflate the radius slightly so we don't try going super close to - // it and hitting it again. - hit_circle.radius(hit_circle.radius() * 1.5f); - obstacles.add(std::make_shared(hit_circle)); - } + Trajectory path = profile_velocity(post_bezier, start.velocity.mag(), goal.velocity.mag(), + motion_constraints, start_time); return path; } @@ -74,7 +55,6 @@ static std::unordered_map> cached_in 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& dynamic_obstacles, const FieldDimensions* field_dimensions, unsigned int robot_id) { // if already on goal, no need to move if (start.position.dist_to(goal.position) < 1e-6) { @@ -127,8 +107,7 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst } // If all else fails, use rrt to ensure obstacle avoidance - return CreatePath::rrt(start, goal, motion_constraints, start_time, static_obstacles, - dynamic_obstacles); + return CreatePath::rrt(start, goal, motion_constraints, start_time, static_obstacles); } std::vector get_intermediates(const LinearMotionInstant& start, diff --git a/src/rj_planning/src/primitives/replanner.cpp b/src/rj_planning/src/primitives/replanner.cpp index 945cfaf5ef..2cf942446a 100644 --- a/src/rj_planning/src/primitives/replanner.cpp +++ b/src/rj_planning/src/primitives/replanner.cpp @@ -25,10 +25,10 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& } Trajectory pre_trajectory = partial_path(previous, params.start.stamp); - Trajectory post_trajectory = CreatePath::intermediate( - pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot, - pre_trajectory.end_time(), params.static_obstacles, params.dynamic_obstacles, - params.field_dimensions, params.robot_id); + Trajectory post_trajectory = + CreatePath::intermediate(pre_trajectory.last().linear_motion(), params.goal, + params.constraints.mot, pre_trajectory.end_time(), + params.static_obstacles, params.field_dimensions, params.robot_id); // If we couldn't profile such that velocity at the end of the partial replan period is valid, // do a full replan. @@ -50,10 +50,9 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& } Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { - Trajectory path = CreatePath::intermediate(params.start.linear_motion(), params.goal, - params.constraints.mot, params.start.stamp, - params.static_obstacles, params.dynamic_obstacles, - params.field_dimensions, params.robot_id); + Trajectory path = CreatePath::intermediate( + params.start.linear_motion(), params.goal, params.constraints.mot, params.start.stamp, + params.static_obstacles, params.field_dimensions, params.robot_id); // if the initial path is empty, the goal must be blocked // try to shift the goal_point until it is no longer blocked @@ -71,10 +70,9 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { almost_goal.position += shift_dir * shift_size; - path = CreatePath::intermediate(params.start.linear_motion(), almost_goal, - params.constraints.mot, params.start.stamp, - params.static_obstacles, params.dynamic_obstacles, - params.field_dimensions, params.robot_id); + path = CreatePath::intermediate( + params.start.linear_motion(), almost_goal, params.constraints.mot, params.start.stamp, + params.static_obstacles, params.field_dimensions, params.robot_id); } if (!path.empty()) { @@ -132,12 +130,8 @@ Trajectory Replanner::create_plan(Replanner::PlanParams params, Trajectory previ RJ::Time hit_time = RJ::Time::max(); - // Use short-circuiting to only check dynamic trajectories if necessary. bool should_partial_replan = - trajectory_hits_static(previous_trajectory, params.static_obstacles, start_time, - &hit_time) || - trajectory_hits_dynamic(previous_trajectory, params.dynamic_obstacles, start_time, nullptr, - &hit_time); + trajectory_hits_static(previous_trajectory, params.static_obstacles, start_time, &hit_time); if (should_partial_replan) { if (hit_time - start_time < partial_replan_lead_time() * 2) { return full_replan(params); diff --git a/src/rj_planning/src/stadium_obstacle.cpp b/src/rj_planning/src/stadium_obstacle.cpp deleted file mode 100644 index 8b760764c5..0000000000 --- a/src/rj_planning/src/stadium_obstacle.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include - -namespace planning { -/** - * 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::StadiumObstacle(rj_geometry::Point pos, rj_geometry::Point vel) { - float scaling = 0.5f; - float width_scaling = 0.1f; - obstacle = std::make_shared(rj_geometry::Circle(pos, kRobotRadius)); - padding = std::make_shared(rj_geometry::StadiumShape( - pos, pos + vel * scaling, 1.5 * kRobotRadius + (vel.mag() * width_scaling))); - velocity = std::make_shared(vel); - shapes.add(obstacle); - shapes.add(dynamic_pointer_cast(padding)->drawshapes()); -} -} // namespace planning \ No newline at end of file diff --git a/src/rj_planning/src/static_obstacle.cpp b/src/rj_planning/src/static_obstacle.cpp deleted file mode 100644 index fda5da2b42..0000000000 --- a/src/rj_planning/src/static_obstacle.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include - -namespace planning { -/** - * 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::StaticObstacle(std::shared_ptr obstacle, - std::shared_ptr padding) { - obstacle = obstacle; - padding = padding; - velocity = std::make_shared(0, 0); - shapes.add(obstacle); - shapes.add(padding); -} - -} // namespace planning \ No newline at end of file diff --git a/src/rj_planning/tests/create_path_test.cpp b/src/rj_planning/tests/create_path_test.cpp index c8b988c3de..0d84855dbf 100644 --- a/src/rj_planning/tests/create_path_test.cpp +++ b/src/rj_planning/tests/create_path_test.cpp @@ -137,7 +137,7 @@ TEST(CreatePath, intermediate_creation_time) { auto start_time = RJ::now(); Trajectory traj = CreatePath::intermediate(start, goal, constraints.mot, RJ::now(), - obstacles, {}, field_dimensions, 0); + obstacles, field_dimensions, 0); double nanos = (RJ::now() - start_time).count(); file << "CreatePath::intermediate() Time: " << nanos / 1e6 << " ms\n"; tfile << "CreatePath::intermediate() Time: "