frc971/autonomous/base_autonomous_actor.cc

#include "frc971/autonomous/base_autonomous_actor.h"

#include <chrono>
#include <cinttypes>
#include <cmath>

#include "aos/logging/logging.h"
#include "aos/util/math.h"
#include "aos/util/phased_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/spline.h"
#include "y2019/control_loops/drivetrain/target_selector_generated.h"

using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace drivetrain = frc971::control_loops::drivetrain;

namespace frc971 {
namespace autonomous {

BaseAutonomousActor::BaseAutonomousActor(
    ::aos::EventLoop *event_loop,
    const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
    : aos::common::actions::ActorBase<Goal>(event_loop, "/autonomous"),
      dt_config_(dt_config),
      initial_drivetrain_({0.0, 0.0}),
      target_selector_hint_sender_(
          event_loop->MakeSender<
              ::y2019::control_loops::drivetrain::TargetSelectorHint>(
              "/drivetrain")),
      drivetrain_goal_sender_(
          event_loop->MakeSender<drivetrain::Goal>("/drivetrain")),
      spline_goal_sender_(
          event_loop->MakeSender<drivetrain::SplineGoal>("/drivetrain")),
      drivetrain_status_fetcher_(
          event_loop->MakeFetcher<drivetrain::Status>("/drivetrain")),
      drivetrain_goal_fetcher_(
          event_loop->MakeFetcher<drivetrain::Goal>("/drivetrain")) {
  event_loop->SetRuntimeRealtimePriority(29);
}

void BaseAutonomousActor::ApplyThrottle(double throttle) {
  goal_spline_handle_ = 0;

  auto builder = drivetrain_goal_sender_.MakeBuilder();

  drivetrain::Goal::Builder goal_builder =
      builder.MakeBuilder<drivetrain::Goal>();
  goal_builder.add_controller_type(drivetrain::ControllerType::POLYDRIVE);
  goal_builder.add_highgear(true);
  goal_builder.add_wheel(0.0);
  goal_builder.add_throttle(throttle);
  builder.CheckOk(builder.Send(goal_builder.Finish()));
}

void BaseAutonomousActor::ResetDrivetrain() {
  AOS_LOG(INFO, "resetting the drivetrain\n");
  max_drivetrain_voltage_ = 12.0;
  goal_spline_handle_ = 0;

  auto builder = drivetrain_goal_sender_.MakeBuilder();

  drivetrain::Goal::Builder goal_builder =
      builder.MakeBuilder<drivetrain::Goal>();
  goal_builder.add_controller_type(drivetrain::ControllerType::POLYDRIVE);
  goal_builder.add_highgear(true);
  goal_builder.add_wheel(0.0);
  goal_builder.add_throttle(0.0);
  goal_builder.add_left_goal(initial_drivetrain_.left);
  goal_builder.add_right_goal(initial_drivetrain_.right);
  goal_builder.add_max_ss_voltage(max_drivetrain_voltage_);
  builder.CheckOk(builder.Send(goal_builder.Finish()));
}

void BaseAutonomousActor::InitializeEncoders() {
  // Spin until we get a message.
  WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });

  initial_drivetrain_.left =
      drivetrain_status_fetcher_->estimated_left_position();
  initial_drivetrain_.right =
      drivetrain_status_fetcher_->estimated_right_position();
}

void BaseAutonomousActor::StartDrive(double distance, double angle,
                                     ProfileParametersT linear,
                                     ProfileParametersT angular) {
  AOS_LOG(INFO, "Driving distance %f, angle %f\n", distance, angle);
  {
    const double dangle = angle * dt_config_.robot_radius;
    initial_drivetrain_.left += distance - dangle;
    initial_drivetrain_.right += distance + dangle;
  }

  auto builder = drivetrain_goal_sender_.MakeBuilder();

  auto linear_offset = ProfileParameters::Pack(*builder.fbb(), &linear);
  auto angular_offset = ProfileParameters::Pack(*builder.fbb(), &angular);

  drivetrain::Goal::Builder goal_builder =
      builder.MakeBuilder<drivetrain::Goal>();

  goal_builder.add_controller_type(drivetrain::ControllerType::MOTION_PROFILE);
  goal_builder.add_highgear(true);
  goal_builder.add_wheel(0.0);
  goal_builder.add_throttle(0.0);
  goal_builder.add_left_goal(initial_drivetrain_.left);
  goal_builder.add_right_goal(initial_drivetrain_.right);
  goal_builder.add_max_ss_voltage(max_drivetrain_voltage_);
  goal_builder.add_linear(linear_offset);
  goal_builder.add_angular(angular_offset);

  builder.CheckOk(builder.Send(goal_builder.Finish()));
}

void BaseAutonomousActor::WaitUntilDoneOrCanceled(
    ::std::unique_ptr<aos::common::actions::Action> action) {
  if (!action) {
    AOS_LOG(ERROR, "No action, not waiting\n");
    return;
  }

  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  while (true) {
    // Poll the running bit and see if we should cancel.
    phased_loop.SleepUntilNext();
    if (!action->Running() || ShouldCancel()) {
      return;
    }
  }
}

bool BaseAutonomousActor::WaitForDriveDone() {
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);

  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (IsDriveDone()) {
      return true;
    }
  }
}

bool BaseAutonomousActor::IsDriveDone() {
  static constexpr double kPositionTolerance = 0.02;
  static constexpr double kVelocityTolerance = 0.10;
  static constexpr double kProfileTolerance = 0.001;

  if (drivetrain_status_fetcher_.get()) {
    if (::std::abs(drivetrain_status_fetcher_->profiled_left_position_goal() -
                   initial_drivetrain_.left) < kProfileTolerance &&
        ::std::abs(drivetrain_status_fetcher_->profiled_right_position_goal() -
                   initial_drivetrain_.right) < kProfileTolerance &&
        ::std::abs(drivetrain_status_fetcher_->estimated_left_position() -
                   initial_drivetrain_.left) < kPositionTolerance &&
        ::std::abs(drivetrain_status_fetcher_->estimated_right_position() -
                   initial_drivetrain_.right) < kPositionTolerance &&
        ::std::abs(drivetrain_status_fetcher_->estimated_left_velocity()) <
            kVelocityTolerance &&
        ::std::abs(drivetrain_status_fetcher_->estimated_right_velocity()) <
            kVelocityTolerance) {
      AOS_LOG(INFO, "Finished drive\n");
      return true;
    }
  }
  return false;
}

bool BaseAutonomousActor::WaitForAboveAngle(double angle) {
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (IsDriveDone()) {
      return true;
    }
    if (drivetrain_status_fetcher_.get()) {
      if (drivetrain_status_fetcher_->ground_angle() > angle) {
        return true;
      }
    }
  }
}

bool BaseAutonomousActor::WaitForBelowAngle(double angle) {
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (IsDriveDone()) {
      return true;
    }
    if (drivetrain_status_fetcher_.get()) {
      if (drivetrain_status_fetcher_->ground_angle() < angle) {
        return true;
      }
    }
  }
}

bool BaseAutonomousActor::WaitForMaxBy(double angle) {
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  double max_angle = -M_PI;
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (IsDriveDone()) {
      return true;
    }
    if (drivetrain_status_fetcher_.get()) {
      if (drivetrain_status_fetcher_->ground_angle() > max_angle) {
        max_angle = drivetrain_status_fetcher_->ground_angle();
      }
      if (drivetrain_status_fetcher_->ground_angle() < max_angle - angle) {
        return true;
      }
    }
  }
}

bool BaseAutonomousActor::WaitForDriveNear(double distance, double angle) {
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  constexpr double kPositionTolerance = 0.02;
  constexpr double kProfileTolerance = 0.001;

  bool drive_has_been_close = false;
  bool turn_has_been_close = false;
  bool printed_first = false;

  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();
    if (drivetrain_status_fetcher_.get()) {
      const double left_profile_error =
          (initial_drivetrain_.left -
           drivetrain_status_fetcher_->profiled_left_position_goal());
      const double right_profile_error =
          (initial_drivetrain_.right -
           drivetrain_status_fetcher_->profiled_right_position_goal());

      const double left_error =
          (initial_drivetrain_.left -
           drivetrain_status_fetcher_->estimated_left_position());
      const double right_error =
          (initial_drivetrain_.right -
           drivetrain_status_fetcher_->estimated_right_position());

      const double profile_distance_to_go =
          (left_profile_error + right_profile_error) / 2.0;
      const double profile_angle_to_go =
          (right_profile_error - left_profile_error) /
          (dt_config_.robot_radius * 2.0);

      const double distance_to_go = (left_error + right_error) / 2.0;
      const double angle_to_go =
          (right_error - left_error) / (dt_config_.robot_radius * 2.0);

      const bool drive_close =
          ::std::abs(profile_distance_to_go) < distance + kProfileTolerance &&
          ::std::abs(distance_to_go) < distance + kPositionTolerance;
      const bool turn_close =
          ::std::abs(profile_angle_to_go) < angle + kProfileTolerance &&
          ::std::abs(angle_to_go) < angle + kPositionTolerance;

      drive_has_been_close |= drive_close;
      turn_has_been_close |= turn_close;
      if (drive_has_been_close && !turn_has_been_close && !printed_first) {
        AOS_LOG(INFO, "Drive finished first\n");
        printed_first = true;
      } else if (!drive_has_been_close && turn_has_been_close &&
                 !printed_first) {
        AOS_LOG(INFO, "Turn finished first\n");
        printed_first = true;
      }

      if (drive_close && turn_close) {
        AOS_LOG(INFO, "Closer than %f < %f distance, %f < %f angle\n",
                distance_to_go, distance, angle_to_go, angle);
        return true;
      }
    }
  }
}

bool BaseAutonomousActor::WaitForDriveProfileNear(double tolerance) {
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();

    const Eigen::Matrix<double, 7, 1> current_error =
        (Eigen::Matrix<double, 7, 1>()
             << initial_drivetrain_.left -
                    drivetrain_status_fetcher_->profiled_left_position_goal(),
         0.0,
         initial_drivetrain_.right -
             drivetrain_status_fetcher_->profiled_right_position_goal(),
         0.0, 0.0, 0.0, 0.0)
            .finished();
    const Eigen::Matrix<double, 2, 1> linear_error =
        dt_config_.LeftRightToLinear(current_error);

    if (drivetrain_status_fetcher_.get()) {
      if (::std::abs(linear_error(0)) < tolerance) {
        AOS_LOG(INFO, "Finished drive\n");
        return true;
      }
    }
  }
}

bool BaseAutonomousActor::WaitForDriveProfileDone() {
  constexpr double kProfileTolerance = 0.001;
  return WaitForDriveProfileNear(kProfileTolerance);
}

bool BaseAutonomousActor::WaitForTurnProfileNear(double tolerance) {
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  while (true) {
    if (ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    drivetrain_status_fetcher_.Fetch();

    const Eigen::Matrix<double, 7, 1> current_error =
        (Eigen::Matrix<double, 7, 1>()
             << initial_drivetrain_.left -
                    drivetrain_status_fetcher_->profiled_left_position_goal(),
         0.0,
         initial_drivetrain_.right -
             drivetrain_status_fetcher_->profiled_right_position_goal(),
         0.0, 0.0, 0.0, 0.0)
            .finished();
    const Eigen::Matrix<double, 2, 1> angular_error =
        dt_config_.LeftRightToAngular(current_error);

    if (drivetrain_status_fetcher_.get()) {
      if (::std::abs(angular_error(0)) < tolerance) {
        AOS_LOG(INFO, "Finished turn\n");
        return true;
      }
    }
  }
}

bool BaseAutonomousActor::WaitForTurnProfileDone() {
  constexpr double kProfileTolerance = 0.001;
  return WaitForTurnProfileNear(kProfileTolerance);
}

double BaseAutonomousActor::DriveDistanceLeft() {
  drivetrain_status_fetcher_.Fetch();
  if (drivetrain_status_fetcher_.get()) {
    const double left_error =
        (initial_drivetrain_.left -
         drivetrain_status_fetcher_->estimated_left_position());
    const double right_error =
        (initial_drivetrain_.right -
         drivetrain_status_fetcher_->estimated_right_position());

    return (left_error + right_error) / 2.0;
  } else {
    return 0;
  }
}

bool BaseAutonomousActor::SplineHandle::SplineDistanceRemaining(
    double distance) {
  base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
  if (base_autonomous_actor_->drivetrain_status_fetcher_.get()) {
    // Confirm that:
    // (a) The spline has started executiong (is_executing remains true even
    //     when we reach the end of the spline).
    // (b) The spline that we are executing is the correct one.
    // (c) There is less than distance distance remaining.
    return base_autonomous_actor_->drivetrain_status_fetcher_
               ->trajectory_logging()
               ->is_executing() &&
           base_autonomous_actor_->drivetrain_status_fetcher_
                   ->trajectory_logging()
                   ->goal_spline_handle() == spline_handle_ &&
           base_autonomous_actor_->drivetrain_status_fetcher_
                   ->trajectory_logging()
                   ->distance_remaining() < distance;
  }
  return false;
}
bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
    double distance) {
  ::aos::time::PhasedLoop phased_loop(
      frc971::controls::kLoopFrequency,
      base_autonomous_actor_->event_loop()->monotonic_now(),
      ActorBase::kLoopOffset);
  while (true) {
    if (base_autonomous_actor_->ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    if (SplineDistanceRemaining(distance)) {
      return true;
    }
  }
}

void BaseAutonomousActor::LineFollowAtVelocity(
    double velocity, y2019::control_loops::drivetrain::SelectionHint hint) {
  {
    auto builder = drivetrain_goal_sender_.MakeBuilder();
    drivetrain::Goal::Builder goal_builder =
        builder.MakeBuilder<drivetrain::Goal>();
    goal_builder.add_controller_type(
        drivetrain::ControllerType::SPLINE_FOLLOWER);
    // TODO(james): Currently the 4.0 is copied from the
    // line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
    // factor it out in some way.
    goal_builder.add_throttle(velocity / 4.0);
    builder.CheckOk(builder.Send(goal_builder.Finish()));
  }

  {
    auto builder = target_selector_hint_sender_.MakeBuilder();
    ::y2019::control_loops::drivetrain::TargetSelectorHint::Builder
        target_hint_builder = builder.MakeBuilder<
            ::y2019::control_loops::drivetrain::TargetSelectorHint>();

    target_hint_builder.add_suggested_target(hint);
    builder.CheckOk(builder.Send(target_hint_builder.Finish()));
  }
}

BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
    std::function<flatbuffers::Offset<frc971::MultiSpline>(
        aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
            *builder)> &&multispline_builder,
    SplineDirection direction) {
  AOS_LOG(INFO, "Planning spline\n");

  int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);

  drivetrain_goal_fetcher_.Fetch();

  auto builder = spline_goal_sender_.MakeBuilder();

  flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
      multispline_builder(&builder);

  drivetrain::SplineGoal::Builder spline_builder =
      builder.MakeBuilder<drivetrain::SplineGoal>();
  spline_builder.add_spline_idx(spline_handle);
  spline_builder.add_drive_spline_backwards(direction ==
                                            SplineDirection::kBackward);
  spline_builder.add_spline(multispline_offset);

  // Calculate the starting position and yaw.
  Eigen::Vector3d start;
  {
    const frc971::MultiSpline *const spline =
        flatbuffers::GetTemporaryPointer(*builder.fbb(), multispline_offset);

    start(0) = spline->spline_x()->Get(0);
    start(1) = spline->spline_y()->Get(0);

    Eigen::Matrix<double, 2, 6> control_points;
    for (size_t ii = 0; ii < 6; ++ii) {
      control_points(0, ii) = spline->spline_x()->Get(ii);
      control_points(1, ii) = spline->spline_y()->Get(ii);
    }

    frc971::control_loops::drivetrain::Spline spline_object(control_points);
    start(2) = spline_object.Theta(0);
    if (direction == SplineDirection::kBackward) {
      start(2) = aos::math::NormalizeAngle(start(2) + M_PI);
    }
  }

  builder.CheckOk(builder.Send(spline_builder.Finish()));

  return BaseAutonomousActor::SplineHandle(spline_handle, this, start);
}

bool BaseAutonomousActor::SplineHandle::IsPlanned() {
  base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
  VLOG(1) << aos::FlatbufferToJson(
      base_autonomous_actor_->drivetrain_status_fetcher_.get());

  if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
      base_autonomous_actor_->drivetrain_status_fetcher_.get()
          ->has_trajectory_logging() &&
      base_autonomous_actor_->drivetrain_status_fetcher_.get()
          ->trajectory_logging()
          ->has_available_splines()) {
    const flatbuffers::Vector<int> *splines =
        base_autonomous_actor_->drivetrain_status_fetcher_.get()
            ->trajectory_logging()
            ->available_splines();

    return std::find(splines->begin(), splines->end(), spline_handle_) !=
           splines->end();
  }
  return false;
}

bool BaseAutonomousActor::SplineHandle::WaitForPlan() {
  ::aos::time::PhasedLoop phased_loop(
      frc971::controls::kLoopFrequency,
      base_autonomous_actor_->event_loop()->monotonic_now(),
      ActorBase::kLoopOffset);
  while (true) {
    if (base_autonomous_actor_->ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    if (IsPlanned()) {
      return true;
    }
  }
}

void BaseAutonomousActor::SplineHandle::Start() {
  auto builder = base_autonomous_actor_->drivetrain_goal_sender_.MakeBuilder();
  drivetrain::Goal::Builder goal_builder =
      builder.MakeBuilder<drivetrain::Goal>();
  goal_builder.add_controller_type(drivetrain::ControllerType::SPLINE_FOLLOWER);

  AOS_LOG(INFO, "Starting spline\n");

  goal_builder.add_spline_handle(spline_handle_);
  base_autonomous_actor_->goal_spline_handle_ = spline_handle_;

  builder.CheckOk(builder.Send(goal_builder.Finish()));
}

bool BaseAutonomousActor::SplineHandle::IsDone() {
  base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();

  // We check that the spline we are waiting on is neither currently planning
  // nor executing (we check is_executed because it is possible to receive
  // status messages with is_executing false before the execution has started).
  // We check for planning so that the user can go straight from starting the
  // planner to executing without a WaitForPlan in between.
  if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
      ((!base_autonomous_actor_->drivetrain_status_fetcher_
             ->trajectory_logging()
             ->is_executed() &&
        base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
                ->current_spline_idx() == spline_handle_) ||
       IsPlanned())) {
    return false;
  }
  return true;
}

bool BaseAutonomousActor::SplineHandle::WaitForDone() {
  ::aos::time::PhasedLoop phased_loop(
      frc971::controls::kLoopFrequency,
      base_autonomous_actor_->event_loop()->monotonic_now(),
      ActorBase::kLoopOffset);
  while (true) {
    if (base_autonomous_actor_->ShouldCancel()) {
      return false;
    }
    phased_loop.SleepUntilNext();
    if (IsDone()) {
      return true;
    }
  }
}

}  // namespace autonomous
}  // namespace frc971