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 Protobuf<?, ?>[] getNested() {
026    return new Protobuf<?, ?>[] {Pose2d.proto};
027  }
028
029  @Override
030  public ProtobufTrajectoryState createMessage() {
031    return ProtobufTrajectoryState.newInstance();
032  }
033
034  @Override
035  public Trajectory.State unpack(ProtobufTrajectoryState msg) {
036    return new Trajectory.State(
037        msg.getTime(),
038        msg.getVelocity(),
039        msg.getAcceleration(),
040        Pose2d.proto.unpack(msg.getPose()),
041        msg.getCurvature());
042  }
043
044  @Override
045  public void pack(ProtobufTrajectoryState msg, Trajectory.State value) {
046    msg.setTime(value.timeSeconds);
047    msg.setVelocity(value.velocityMetersPerSecond);
048    msg.setAcceleration(value.accelerationMetersPerSecondSq);
049    Pose2d.proto.pack(msg.getMutablePose(), value.poseMeters);
050    msg.setCurvature(value.curvatureRadPerMeter);
051  }
052}