Merge branch 'ros2' into ball-placement-obstacle

soccer/src/soccer/CMakeLists.txt

@@ -40,6 +40,7 @@ set(ROBOCUP_LIB_SRC
     planning/planner/path_target_path_planner.cpp
     planning/planner/pivot_path_planner.cpp
     planning/planner/line_pivot_path_planner.cpp
+    planning/planner/rotate_path_planner.cpp
     planning/planner/plan_request.cpp
     planning/planner/settle_path_planner.cpp
     planning/planner/goalie_idle_path_planner.cpp

soccer/src/soccer/control/trapezoidal_motion.cpp

@@ -112,9 +112,9 @@ double Trapezoidal::get_time(double distance, double path_length, double max_spe
     }
 }
 
-bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap,
-                        double start_speed, double final_speed, double& pos_out,
-                        double& speed_out) {
+bool Trapezoidal::trapezoidal_motion(double path_length, double max_speed, double max_acc,
+                                     double time_into_lap, double start_speed, double final_speed,
+                                     double& pos_out, double& speed_out) {
     // begin by assuming that there's enough time to get up to full speed
     // we do this by calculating the full ramp-up and ramp-down, then seeing
     // if the distance travelled is too great.  If it's gone too far, this is

soccer/src/soccer/control/trapezoidal_motion.hpp

@@ -1,23 +1,5 @@
 #pragma once
 
-/**
- * This function evaluates a control situation using "bang-bang". The robot
- * accelerates at maxAcceleration until it hits max speed, then decelerates at
- * max acc to the end point.
- *
- * @param[in] 	pathLength The total distance we are trying to travel
- * @param[in] 	timeIntoLap How long since we started moving along the trapezoid
- * @param[in] 	finalSpeed The speed we'd like to be going at the end of the
- * trapezoid
- * @param[out] 	distOut The distance to be at at the given time
- * @param[out] 	speedOut The speed to be at at the given time
- * @return		true if the trapezoid is valid at the given time, false
- * otherwise
- */
-bool trapezoidal_motion(double path_length, double max_speed, double max_acc,
-                       double time_into_lap, double start_speed, double final_speed,
-                       double& pos_out, double& speed_out);
-
 namespace Trapezoidal {
 /**
  * Estimates how long it would take to move to a certain distance down a path
@@ -37,4 +19,21 @@ namespace Trapezoidal {
 double get_time(double distance, double path_length, double max_speed,
                double max_acc, double start_speed, double final_speed);
 
+/**
+ * This function evaluates a control situation using "bang-bang". The robot
+ * accelerates at maxAcceleration until it hits max speed, then decelerates at
+ * max acc to the end point.
+ *
+ * @param[in] 	pathLength The total distance we are trying to travel
+ * @param[in] 	timeIntoLap How long since we started moving along the trapezoid
+ * @param[in] 	finalSpeed The speed we'd like to be going at the end of the
+ * trapezoid
+ * @param[out] 	distOut The distance to be at at the given time
+ * @param[out] 	speedOut The speed to be at at the given time
+ * @return		true if the trapezoid is valid at the given time, false
+ * otherwise
+ */
+bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap,
+                        double start_speed, double final_speed, double& pos_out, double& speed_out);
+
 }  // namespace Trapezoidal

soccer/src/soccer/control/trapezoidal_motion_test.cpp

@@ -16,8 +16,8 @@ bool trapezoid1(double t, double& pos_out, double& speed_out) {
     double start_speed = 0;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         auto time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -34,8 +34,8 @@ bool trapezoid2(double t, double& pos_out, double& speed_out) {
     double start_speed = 1;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -55,8 +55,8 @@ bool triangle1(double t, double& pos_out, double& speed_out) {
     double start_speed = 0;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -76,8 +76,8 @@ bool triangle2(double t, double& pos_out, double& speed_out) {
     double start_speed = 0;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -97,8 +97,8 @@ bool triangle3(double t, double& pos_out, double& speed_out) {
     double start_speed = 1;
     double final_speed = 1;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -118,8 +118,8 @@ bool triangle4(double t, double& pos_out, double& speed_out) {
     double start_speed = 1;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,

soccer/src/soccer/planning/global_state.cpp

@@ -74,6 +74,10 @@ GlobalState::GlobalState(rclcpp::Node* node) {
     auto lock = std::lock_guard(last_world_state_mutex_);
     return &last_world_state_;
 }
+[[nodiscard]] const FieldDimensions* GlobalState::field_dimensions() const {
+    auto lock = std::lock_guard(last_field_dimensions_mutex_);
+    return &last_field_dimensions_;
+}
 
 rj_geometry::ShapeSet GlobalState::create_defense_area_obstacles() {
     // need field dimensions and to be initialized for this to

soccer/src/soccer/planning/global_state.hpp

@@ -34,6 +34,7 @@ class GlobalState {
     [[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;
 
 private:
     rclcpp::Subscription<rj_msgs::msg::PlayState>::SharedPtr play_state_sub_;

soccer/src/soccer/planning/planner/collect_path_planner.cpp

@@ -224,8 +224,10 @@ Trajectory CollectPathPlanner::coarse_approach(
                                  target_slow,
                                  static_obstacles,
                                  dynamic_obstacles,
+                                 plan_request.field_dimensions,
                                  plan_request.constraints,
-                                 AngleFns::face_point(ball.position)};
+                                 AngleFns::face_point(ball.position),
+                                 plan_request.shell_id};
     Trajectory coarse_path = Replanner::create_plan(params, previous_);
 
     if (plan_request.debug_drawer != nullptr) {
@@ -282,8 +284,10 @@ Trajectory CollectPathPlanner::fine_approach(
                                  target_hit,
                                  static_obstacles,
                                  dynamic_obstacles,
+                                 plan_request.field_dimensions,
                                  plan_request.constraints,
-                                 AngleFns::face_point(ball.position)};
+                                 AngleFns::face_point(ball.position),
+                                 plan_request.shell_id};
     Trajectory path_hit = Replanner::create_plan(params, previous_);
 
     path_hit.set_debug_text("fine");
@@ -376,8 +380,10 @@ Trajectory CollectPathPlanner::control(const PlanRequest& plan_request, RobotIns
                                  target,
                                  static_obstacles,
                                  dynamic_obstacles,
+                                 plan_request.field_dimensions,
                                  plan_request.constraints,
-                                 AngleFns::face_point(ball.position)};
+                                 AngleFns::face_point(ball.position),
+                                 plan_request.shell_id};
 
     Trajectory path = Replanner::create_plan(params, previous_);
 
@@ -419,10 +425,14 @@ Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request,
     // programmatically
     LinearMotionInstant target{plan_request.start.position(), Point()};
 
-    Replanner::PlanParams params{
-        plan_request.start,       target,
-        static_obstacles,         dynamic_obstacles,
-        plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position)};
+    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),
+                                 plan_request.shell_id};
     Trajectory path = Replanner::create_plan(params, previous_);
     path.set_debug_text("Invalid state in collect");
 

soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp

@@ -31,7 +31,8 @@ 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,
-                              plan_request.constraints, angle_function, RJ::Seconds(3.0)},
+                              plan_request.field_dimensions, plan_request.constraints,
+                              angle_function, plan_request.shell_id, RJ::Seconds(3.0)},
         std::move(previous_));
 
     // Debug drawing

soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp

@@ -51,6 +51,7 @@ LinePivotPathPlanner::State LinePivotPathPlanner::next_state(const PlanRequest&
     double vel = plan_request.world_state->get_robot(true, static_cast<int>(plan_request.shell_id))
                      .velocity.linear()
                      .mag();
+
     if (current_state_ == LINE && (target_point.dist_to(current_point) < 0.3) && (vel < 0.3) &&
         (!plan_request.play_state.is_stop())) {
         return PIVOT;

soccer/src/soccer/planning/planner/path_target_path_planner.cpp

@@ -41,7 +41,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, dynamic_obstacles,
-                              request.constraints, angle_function, RJ::Seconds(3.0)},
+                              request.field_dimensions, request.constraints, angle_function,
+                              request.shell_id, RJ::Seconds(3.0)},
         std::move(previous_));
 
     previous_ = trajectory;
@@ -87,6 +88,10 @@ AngleFunction PathTargetPathPlanner::get_angle_function(const PlanRequest& reque
         return AngleFns::face_angle(std::get<FaceAngle>(face_option).target);
     }
 
+    if (std::holds_alternative<FaceTarget>(face_option)) {
+        return AngleFns::face_point(request.motion_command.target.position);
+    }
+
     // default to facing tangent to path
     // (rj_convert in motion_command.hpp also follows this default)
     return AngleFns::tangent;

soccer/src/soccer/planning/planner/plan_request.hpp

@@ -30,8 +30,9 @@ struct PlanRequest {
                 RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles,
                 rj_geometry::ShapeSet virtual_obstacles, TrajectoryCollection* planned_trajectories,
                 unsigned shell_id, const WorldState* world_state, PlayState play_state,
-                int8_t priority = 0, rj_drawing::RosDebugDrawer* debug_drawer = nullptr,
-                bool ball_sense = false, float min_dist_from_ball = 0, float dribbler_speed = 0)
+                const FieldDimensions* field_dimensions, int8_t priority = 0,
+                rj_drawing::RosDebugDrawer* debug_drawer = nullptr, bool ball_sense = false,
+                float min_dist_from_ball = 0, float dribbler_speed = 0)
         : start(start),
           motion_command(command),  // NOLINT
           constraints(constraints),
@@ -42,6 +43,7 @@ struct PlanRequest {
           world_state(world_state),
           priority(priority),
           play_state(play_state),
+          field_dimensions(field_dimensions),
           debug_drawer(debug_drawer),
           ball_sense(ball_sense),
           min_dist_from_ball(min_dist_from_ball),
@@ -102,6 +104,11 @@ struct PlanRequest {
      */
     PlayState play_state;
 
+    /**
+     * the Field Dimensions
+     */
+    const FieldDimensions* field_dimensions;
+
     /**
      * Allows debug drawing in the world. If this is nullptr, no debug drawing
      * should be performed.

soccer/src/soccer/planning/planner/rotate_path_planner.cpp

@@ -0,0 +1,80 @@
+#include "rotate_path_planner.hpp"
+
+#include <memory>
+#include <vector>
+
+#include <rj_constants/constants.hpp>
+#include <rj_geometry/pose.hpp>
+#include <rj_geometry/util.hpp>
+
+#include "planning/instant.hpp"
+#include "planning/planning_params.hpp"
+#include "planning/primitives/angle_planning.hpp"
+#include "planning/primitives/path_smoothing.hpp"
+#include "planning/primitives/trapezoidal_motion.hpp"
+#include "planning/primitives/velocity_profiling.hpp"
+#include "planning/trajectory.hpp"
+
+namespace planning {
+using namespace rj_geometry;
+
+Trajectory RotatePathPlanner::plan(const PlanRequest& request) {
+    return pivot(request);  // type is Trajectory
+}
+
+bool RotatePathPlanner::is_done() const {
+    if (!cached_angle_change_) {
+        return false;
+    }
+    return abs(cached_angle_change_.value()) <
+           degrees_to_radians(static_cast<float>(kIsDoneAngleChangeThresh));
+}
+
+Trajectory RotatePathPlanner::pivot(const PlanRequest& request) {
+    const RobotInstant& start_instant = request.start;
+    const auto& linear_constraints = request.constraints.mot;
+    const auto& rotation_constraints = request.constraints.rot;
+
+    rj_geometry::ShapeSet static_obstacles;
+    std::vector<DynamicObstacle> dynamic_obstacles;
+    fill_obstacles(request, &static_obstacles, &dynamic_obstacles, false);
+
+    const MotionCommand& command = request.motion_command;
+
+    auto pivot_point =
+        request.world_state->get_robot(true, static_cast<int>(request.shell_id)).pose.position();
+    auto pivot_target = command.target.position;
+
+    double start_angle =
+        request.world_state->get_robot(true, static_cast<int>(request.shell_id)).pose.heading();
+
+    double target_angle = pivot_point.angle_to(pivot_target);
+    double angle_change = fix_angle_radians(target_angle - start_angle);
+
+    cached_angle_change_ = angle_change;
+
+    Trajectory path{};
+
+    if (abs(*cached_target_angle_ - target_angle) < degrees_to_radians(kIsDoneAngleChangeThresh)) {
+        if (cached_path_) {
+            path = cached_path_.value();
+        } else {
+            plan_angles(&path, start_instant, AngleFns::face_point(pivot_target),
+                        request.constraints.rot);
+            path.stamp(RJ::now());
+            cached_path_ = path;
+        }
+    } else {
+        cached_path_.reset();
+        plan_angles(&path, start_instant, AngleFns::face_point(pivot_target),
+                    request.constraints.rot);
+        path.stamp(RJ::now());
+        cached_path_ = path;
+    }
+
+    cached_target_angle_ = target_angle;
+
+    return path;
+}
+
+}  // namespace planning