001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory.proto;
006
007import edu.wpi.first.math.geometry.Pose2d;
008import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState;
009import edu.wpi.first.math.trajectory.Trajectory;
010import edu.wpi.first.util.protobuf.Protobuf;
011import us.hebi.quickbuf.Descriptors.Descriptor;
012
013public class TrajectoryStateProto implements Protobuf<Trajectory.State, ProtobufTrajectoryState> {
014  @Override
015  public Class<Trajectory.State> getTypeClass() {
016    return Trajectory.State.class;
017  }
018
019  @Override
020  public Descriptor getDescriptor() {
021    return ProtobufTrajectoryState.getDescriptor();
022  }
023
024  @Override
025  public ProtobufTrajectoryState createMessage() {
026    return ProtobufTrajectoryState.newInstance();
027  }
028
029  @Override
030  public Trajectory.State unpack(ProtobufTrajectoryState msg) {
031    return new Trajectory.State(
032        msg.getTime(),
033        msg.getVelocity(),
034        msg.getAcceleration(),
035        Pose2d.proto.unpack(msg.getPose()),
036        msg.getCurvature());
037  }
038
039  @Override
040  public void pack(ProtobufTrajectoryState msg, Trajectory.State value) {
041    msg.setTime(value.timeSeconds);
042    msg.setVelocity(value.velocityMetersPerSecond);
043    msg.setAcceleration(value.accelerationMetersPerSecondSq);
044    Pose2d.proto.pack(msg.getMutablePose(), value.poseMeters);
045    msg.setCurvature(value.curvatureRadPerMeter);
046  }
047}