Resolving merge conflicts

rj_msgs/msg/MotionCommand.msg

@@ -5,4 +5,4 @@ rj_geometry_msgs/Point[<=1] override_face_point
 bool[<=1] face_ball
 bool[<=1] ignore_ball
 rj_geometry_msgs/Point[<=1] pivot_point
-
+float64[<=1] pivot_radius

soccer/src/soccer/CMakeLists.txt

@@ -39,6 +39,7 @@ set(ROBOCUP_LIB_SRC
  planning/planner/line_kick_path_planner.cpp
  planning/planner/path_target_path_planner.cpp
  planning/planner/pivot_path_planner.cpp
+ planning/planner/line_pivot_path_planner.cpp
  planning/planner/plan_request.cpp
  planning/planner/settle_path_planner.cpp
  planning/planner/goalie_idle_path_planner.cpp
@@ -91,6 +92,8 @@ set(ROBOCUP_LIB_SRC
  strategy/agent/position/penalty_non_kicker.cpp
  strategy/agent/position/smartidling.cpp
  strategy/agent/position/zoner.cpp
+ strategy/agent/position/pivot_test.cpp
+
  )
 
 set(SOCCER_TEST_SRC

soccer/src/soccer/game_state.hpp

@@ -151,7 +151,7 @@ struct PlayState {
  ball_placement_point() == other.ball_placement_point();
  }
 
- bool same_as(const PlayState& other) const {
+ [[nodiscard]] bool same_as(const PlayState& other) const {
  return state_ == other.state_ && restart_ == other.restart_ &&
  our_restart_ == other.our_restart_;
  }

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

@@ -0,0 +1,144 @@
+#include "line_pivot_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 LinePivotPathPlanner::plan(const PlanRequest& request) {
+ current_state_ = next_state(request);
+ Trajectory path;
+
+ if (current_state_ == LINE) {
+ path = line(request);
+ } else {
+ path = pivot(request);
+ }
+
+ return path;
+}
+
+bool LinePivotPathPlanner::is_done() const {
+ if (!cached_angle_change_.has_value()) {
+ return false;
+ }
+ return abs(cached_angle_change_.value()) <
+ degrees_to_radians(static_cast<float>(kIsDoneAngleChangeThresh));
+}
+
+LinePivotPathPlanner::State LinePivotPathPlanner::next_state(const PlanRequest& plan_request) {
+ const MotionCommand& command = plan_request.motion_command;
+ auto current_point =
+ plan_request.world_state->get_robot(true, static_cast<int>(plan_request.shell_id))
+ .pose.position();
+ auto pivot_point = command.pivot_point;
+
+ double dist_from_point = command.pivot_radius;
+ auto target_point = pivot_point + (current_point - pivot_point).normalized(dist_from_point);
+ 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)) {
+ return PIVOT;
+ }
+ if (current_state_ == PIVOT && (pivot_point.dist_to(current_point) > (dist_from_point * 5))) {
+ return LINE;
+ }
+ return current_state_;
+}
+
+Trajectory LinePivotPathPlanner::line(const PlanRequest& plan_request) {
+ const MotionCommand& command = plan_request.motion_command;
+ auto pivot_point = command.pivot_point;
+ auto current_point =
+ plan_request.world_state->get_robot(true, static_cast<int>(plan_request.shell_id))
+ .pose.position();
+
+ double dist_from_point = command.pivot_radius;
+ auto target_point = pivot_point + (current_point - pivot_point).normalized(dist_from_point);
+
+ // Create an updated MotionCommand and forward to PathTargetPathPlaner
+ PlanRequest modified_request = plan_request;
+
+ LinearMotionInstant target{target_point};
+
+ MotionCommand modified_command{"path_target", target};
+ modified_request.motion_command = modified_command;
+
+ return path_target_.plan(modified_request);
+}
+
+Trajectory LinePivotPathPlanner::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;
+
+ double radius = pivot::PARAM_radius_multiplier * command.pivot_radius;
+ auto pivot_point = command.pivot_point;
+ auto pivot_target = command.target.position;
+
+ auto final_position = pivot_point + (pivot_point - pivot_target).normalized(radius);
+ std::vector<Point> points;
+
+ // max_speed = max_radians * radius
+ MotionConstraints new_constraints = request.constraints.mot;
+ new_constraints.max_speed =
+ std::min(new_constraints.max_speed, rotation_constraints.max_speed * radius) * .5;
+
+ double start_angle = pivot_point.angle_to(
+ request.world_state->get_robot(true, static_cast<int>(request.shell_id)).pose.position());
+ double target_angle = pivot_point.angle_to(final_position);
+ double angle_change = fix_angle_radians(target_angle - start_angle);
+
+ cached_angle_change_ = angle_change;
+
+ constexpr double kMaxInterpolationRadians = 10 * M_PI / 180;
+ const int interpolations = std::ceil(std::abs(angle_change) / kMaxInterpolationRadians);
+
+ points.push_back(start_instant.position());
+ for (int i = 1; i <= interpolations; i++) {
+ double percent = (double)i / interpolations;
+ double angle = start_angle + angle_change * percent;
+ Point point = Point::direction(angle).normalized(radius) + pivot_point;
+ points.push_back(point);
+ }
+
+ BezierPath path_bezier(points, Point(0, 0), Point(0, 0), linear_constraints);
+
+ Trajectory path = profile_velocity(path_bezier, start_instant.linear_velocity().mag(), 0,
+ linear_constraints, start_instant.stamp);
+ if (!Twist::nearly_equals(path.last().velocity, Twist::zero())) {
+ RobotInstant last;
+ last.position() = final_position;
+ last.velocity = Twist::zero();
+ last.stamp = path.end_time() + 100ms;
+ path.append_instant(last);
+ }
+ path.hold_for(RJ::Seconds(3.0));
+
+ plan_angles(&path, start_instant, AngleFns::face_point(pivot_point), request.constraints.rot);
+ path.stamp(RJ::now());
+
+ return path;
+}
+
+} // namespace planning

soccer/src/soccer/planning/planner/line_pivot_path_planner.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include "path_planner.hpp"
+#include "path_target_path_planner.hpp"
+
+namespace planning {
+
+/**
+ * PathPlanner which pivots about the point given by <command.pivot_point> to the target point
+ * <command.target.position>.
+ *
+ * For instance, if <command.pivot_point> = ball.position and the ball is in
+ * the robot's mouth, then the robot will pivot while maintaining contact with
+ * the ball to <command.target.position>.
+ *
+ * Params taken from MotionCommand:
+ * target.pivot_point - robot will pivot about this point
+ * target.position - robot will face this point when done
+ */
+class LinePivotPathPlanner : public PathPlanner {
+public:
+ LinePivotPathPlanner() : PathPlanner("line_pivot") {}
+ ~LinePivotPathPlanner() override = default;
+
+ LinePivotPathPlanner(LinePivotPathPlanner&&) noexcept = default;
+ LinePivotPathPlanner& operator=(LinePivotPathPlanner&&) noexcept = default;
+ LinePivotPathPlanner(const LinePivotPathPlanner&) = default;
+ LinePivotPathPlanner& operator=(const LinePivotPathPlanner&) = default;
+
+ Trajectory plan(const PlanRequest& request) override;
+
+ void reset() override {
+ cached_angle_change_ = std::nullopt;
+ current_state_ = LINE;
+ }
+ [[nodiscard]] bool is_done() const override;
+
+private:
+ Trajectory previous_;
+
+ // cache the most recent angle change so we know when we're done
+ std::optional<double> cached_angle_change_;
+
+ PathTargetPathPlanner path_target_{};
+
+ Trajectory line(const PlanRequest& request);
+ Trajectory pivot(const PlanRequest& request);
+
+ // TODO(Kevin): ros param this
+ static constexpr double kIsDoneAngleChangeThresh{1.0};
+
+ enum State { LINE, PIVOT };
+
+ State next_state(const PlanRequest& request);
+
+ State current_state_ = LINE;
+};
+} // namespace planning

soccer/src/soccer/planning/planner/motion_command.hpp

@@ -57,6 +57,7 @@ struct MotionCommand {
  PathTargetFaceOption face_option = FaceTarget{};
  bool ignore_ball{false};
  rj_geometry::Point pivot_point{};
+ double pivot_radius{kRobotRadius};
 };
 bool operator==(const MotionCommand& a, const MotionCommand& b);
 
@@ -109,6 +110,9 @@ struct RosConverter<planning::MotionCommand, rj_msgs::msg::MotionCommand> {
  // convert pivot point
  result.pivot_point.push_back(convert_to_ros(from.pivot_point));
 
+ // convert the pivot radius
+ result.pivot_radius.push_back(from.pivot_radius);
+
  return result;
  }
 
@@ -146,6 +150,10 @@ struct RosConverter<planning::MotionCommand, rj_msgs::msg::MotionCommand> {
  result.pivot_point = convert_from_ros(from.pivot_point[0]);
  }
 
+ if (!from.pivot_radius.empty()) {
+ result.pivot_radius = from.pivot_radius[0];
+ }
+
  return result;
  }
 };

soccer/src/soccer/planning/planner/pivot_path_planner.cpp

@@ -41,16 +41,16 @@ Trajectory PivotPathPlanner::plan(const PlanRequest& request) {
  cached_pivot_point_.has_value() &&
  cached_pivot_point_.value().dist_to(pivot_point) < kRobotMouthWidth / 2;
 
- if (pivot_target_unchanged && pivot_point_unchanged) {
- return previous_;
- }
+ // if (pivot_target_unchanged && pivot_point_unchanged) {
+ //  return previous_;
+ // }
 
- if (pivot_point_unchanged) {
- pivot_point = *cached_pivot_point_;
- }
- if (pivot_target_unchanged) {
- pivot_target = *cached_pivot_target_;
- }
+ // if (pivot_point_unchanged) {
+ //  pivot_point = *cached_pivot_point_;
+ // }
+ // if (pivot_target_unchanged) {
+ //  pivot_target = *cached_pivot_target_;
+ // }
 
  cached_pivot_target_ = pivot_target;
  cached_pivot_point_ = pivot_point;
@@ -105,7 +105,9 @@ bool PivotPathPlanner::is_done() const {
  if (!cached_angle_change_.has_value()) {
  return false;
  }
- return cached_angle_change_.value() < IS_DONE_ANGLE_CHANGE_THRESH;
+ bool val = abs(cached_angle_change_.value()) <
+ degrees_to_radians(static_cast<float>(kIsDoneAngleChangeThresh));
+ return val;
 }
 
 } // namespace planning

soccer/src/soccer/planning/planner/pivot_path_planner.hpp

@@ -47,6 +47,6 @@ class PivotPathPlanner : public PathPlanner {
  std::optional<double> cached_angle_change_;
 
  // TODO(Kevin): ros param this
- double IS_DONE_ANGLE_CHANGE_THRESH = 1.0;
+ static constexpr double kIsDoneAngleChangeThresh{1.0};
 };
 } // namespace planning

soccer/src/soccer/planning/planner_for_robot.cpp

@@ -6,6 +6,7 @@
 #include "planning/planner/goalie_idle_path_planner.hpp"
 #include "planning/planner/intercept_path_planner.hpp"
 #include "planning/planner/line_kick_path_planner.hpp"
+#include "planning/planner/line_pivot_path_planner.hpp"
 #include "planning/planner/path_target_path_planner.hpp"
 #include "planning/planner/pivot_path_planner.hpp"
 #include "planning/planner/settle_path_planner.hpp"
@@ -30,6 +31,7 @@ PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node,
  path_planners_[CollectPathPlanner().name()] = std::make_unique<CollectPathPlanner>();
  path_planners_[LineKickPathPlanner().name()] = std::make_unique<LineKickPathPlanner>();
  path_planners_[PivotPathPlanner().name()] = std::make_unique<PivotPathPlanner>();
+ path_planners_[LinePivotPathPlanner().name()] = std::make_unique<LinePivotPathPlanner>();
  path_planners_[EscapeObstaclesPathPlanner().name()] =
  std::make_unique<EscapeObstaclesPathPlanner>();
 
@@ -130,7 +132,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) {
  max_dribbler_speed = 0;
  break;
  case PlayState::State::Setup:
- // TODO: this is a hacky solution for us to stop kicking the ball by
+ // TODO(jacksherling): this is a hacky solution for us to stop kicking the ball by
  // accident in kickoff, not an actual league rule
  min_dist_from_ball = 0.2;
  max_robot_speed = 10.0;