diff --git a/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp index 5fe63d0606..2ea8237234 100644 --- a/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp +++ b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp @@ -9,6 +9,8 @@ #include #include +#include "frc/trajectory/Trajectory.h" + using namespace frc; FieldObject2d::FieldObject2d(FieldObject2d&& rhs) { @@ -53,6 +55,16 @@ void FieldObject2d::SetPoses(std::initializer_list poses) { SetPoses(wpi::makeArrayRef(poses.begin(), poses.end())); } +void FieldObject2d::SetTrajectory(const Trajectory& trajectory) { + std::scoped_lock lock(m_mutex); + m_poses.clear(); + m_poses.reserve(trajectory.States().size()); + for (auto&& state : trajectory.States()) { + m_poses.push_back(state.pose); + } + UpdateEntry(); +} + std::vector FieldObject2d::GetPoses() const { std::scoped_lock lock(m_mutex); UpdateFromEntry(); diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h index 0319cbabd3..eb3625181c 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h @@ -21,6 +21,7 @@ namespace frc { class Field2d; +class Trajectory; /** * Game field object on a Field2d. @@ -76,6 +77,13 @@ class FieldObject2d { */ void SetPoses(std::initializer_list poses); + /** + * Sets poses from a trajectory. + * + * @param trajectory The trajectory from which poses should be added. + */ + void SetTrajectory(const Trajectory& trajectory); + /** * Get multiple poses. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java index a5785df58f..aee7e13dac 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java @@ -8,6 +8,7 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; import java.nio.ByteBuffer; import java.nio.ByteOrder; import java.util.ArrayList; @@ -84,6 +85,19 @@ public class FieldObject2d { updateEntry(); } + /** + * Sets poses from a trajectory. + * + * @param trajectory The trajectory from which the poses should be added. + */ + public synchronized void setTrajectory(Trajectory trajectory) { + m_poses.clear(); + for (Trajectory.State state : trajectory.getStates()) { + m_poses.add(state.poseMeters); + } + updateEntry(); + } + /** * Get multiple poses. *