mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpilib] Move Field2d to SmartDashboard
This commit is contained in:
Peter Johnson
@@ -24,7 +24,7 @@ import edu.wpi.first.wpiutil.math.numbers.N7;
|
||||
/**
|
||||
* This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with
|
||||
* {@link #setInputs(double, double)}, call {@link #update(double)} to update the simulation,
|
||||
* and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.simulation.Field2d} to
|
||||
* and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to
|
||||
* visualize their robot on the Sim GUI's field.
|
||||
*
|
||||
* <p>Our state-space system is:
|
||||
|
||||
@@ -1,100 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.simulation;
|
||||
|
||||
import edu.wpi.first.hal.SimDevice;
|
||||
import edu.wpi.first.hal.SimDouble;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
|
||||
/**
|
||||
* 2D representation of game field (for simulation).
|
||||
*
|
||||
* <p>In non-simulation mode this simply stores and returns the robot pose.
|
||||
*
|
||||
* <p>The robot pose is the actual location shown on the simulation view.
|
||||
* This may or may not match the robot's internal odometry. For example, if
|
||||
* the robot is shown at a particular starting location, the pose in this
|
||||
* class would represent the actual location on the field, but the robot's
|
||||
* internal state might have a 0,0,0 pose (unless it's initialized to
|
||||
* something different).
|
||||
*
|
||||
* <p>As the user is able to edit the pose, code performing updates should get
|
||||
* the robot pose, transform it as appropriate (e.g. based on simulated wheel
|
||||
* velocity), and set the new pose.
|
||||
*/
|
||||
public class Field2d {
|
||||
public Field2d() {
|
||||
m_device = SimDevice.create("Field2D");
|
||||
if (m_device != null) {
|
||||
m_x = m_device.createDouble("x", false, 0.0);
|
||||
m_y = m_device.createDouble("y", false, 0.0);
|
||||
m_rot = m_device.createDouble("rot", false, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the robot pose from a Pose object.
|
||||
*
|
||||
* @param pose 2D pose
|
||||
*/
|
||||
public void setRobotPose(Pose2d pose) {
|
||||
if (m_device != null) {
|
||||
Translation2d translation = pose.getTranslation();
|
||||
m_x.set(translation.getX());
|
||||
m_y.set(translation.getY());
|
||||
m_rot.set(pose.getRotation().getDegrees());
|
||||
} else {
|
||||
m_pose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the robot pose from x, y, and rotation.
|
||||
*
|
||||
* @param xMeters X location, in meters
|
||||
* @param yMeters Y location, in meters
|
||||
* @param rotation rotation
|
||||
*/
|
||||
public void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
|
||||
if (m_device != null) {
|
||||
m_x.set(xMeters);
|
||||
m_y.set(yMeters);
|
||||
m_rot.set(rotation.getDegrees());
|
||||
} else {
|
||||
m_pose = new Pose2d(xMeters, yMeters, rotation);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the robot pose.
|
||||
*
|
||||
* @return 2D pose
|
||||
*/
|
||||
public Pose2d getRobotPose() {
|
||||
if (m_device != null) {
|
||||
return new Pose2d(m_x.get(), m_y.get(), Rotation2d.fromDegrees(m_rot.get()));
|
||||
} else {
|
||||
return m_pose;
|
||||
}
|
||||
}
|
||||
|
||||
private Pose2d m_pose;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private final SimDevice m_device;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private SimDouble m_x;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private SimDouble m_y;
|
||||
|
||||
private SimDouble m_rot;
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.smartdashboard;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.wpilibj.Sendable;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
|
||||
/**
|
||||
* 2D representation of game field for dashboards.
|
||||
*
|
||||
* <p>An object's pose is the location shown on the dashboard view. Note that
|
||||
* for the robot, this may or may not match the internal odometry. For example,
|
||||
* the robot is shown at a particular starting location, the pose in this
|
||||
* class would represent the actual location on the field, but the robot's
|
||||
* internal state might have a 0,0,0 pose (unless it's initialized to
|
||||
* something different).
|
||||
*
|
||||
* <p>As the user is able to edit the pose, code performing updates should get
|
||||
* the robot pose, transform it as appropriate (e.g. based on wheel odometry),
|
||||
* and set the new pose.
|
||||
*
|
||||
* <p>This class provides methods to set the robot pose, but other objects can
|
||||
* also be shown by using the getObject() function. Other objects can
|
||||
* also have multiple poses (which will show the object at multiple locations).
|
||||
*/
|
||||
public class Field2d implements Sendable {
|
||||
/**
|
||||
* Constructor.
|
||||
*/
|
||||
public Field2d() {
|
||||
FieldObject2d obj = new FieldObject2d("Robot");
|
||||
obj.setPose(new Pose2d());
|
||||
m_objects.add(obj);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the robot pose from a Pose object.
|
||||
*
|
||||
* @param pose 2D pose
|
||||
*/
|
||||
public synchronized void setRobotPose(Pose2d pose) {
|
||||
m_objects.get(0).setPose(pose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the robot pose from x, y, and rotation.
|
||||
*
|
||||
* @param xMeters X location, in meters
|
||||
* @param yMeters Y location, in meters
|
||||
* @param rotation rotation
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
|
||||
m_objects.get(0).setPose(xMeters, yMeters, rotation);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the robot pose.
|
||||
*
|
||||
* @return 2D pose
|
||||
*/
|
||||
public synchronized Pose2d getRobotPose() {
|
||||
return m_objects.get(0).getPose();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get or create a field object.
|
||||
*
|
||||
* @return Field object
|
||||
*/
|
||||
public synchronized FieldObject2d getObject(String name) {
|
||||
for (FieldObject2d obj : m_objects) {
|
||||
if (obj.m_name == name) {
|
||||
return obj;
|
||||
}
|
||||
}
|
||||
FieldObject2d obj = new FieldObject2d(name);
|
||||
m_objects.add(obj);
|
||||
if (m_table != null) {
|
||||
synchronized (obj) {
|
||||
obj.m_entry = m_table.getEntry(name);
|
||||
}
|
||||
}
|
||||
return obj;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the robot object.
|
||||
*
|
||||
* @return Field object for robot
|
||||
*/
|
||||
public synchronized FieldObject2d getRobotObject() {
|
||||
return m_objects.get(0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("Field2d");
|
||||
m_table = builder.getTable();
|
||||
|
||||
synchronized (this) {
|
||||
for (FieldObject2d obj : m_objects) {
|
||||
synchronized (obj) {
|
||||
obj.m_entry = m_table.getEntry(obj.m_name);
|
||||
obj.updateEntry(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private NetworkTable m_table;
|
||||
private final List<FieldObject2d> m_objects = new ArrayList<>();
|
||||
}
|
||||
@@ -0,0 +1,153 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.smartdashboard;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
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;
|
||||
|
||||
/**
|
||||
* Game field object on a Field2d.
|
||||
*/
|
||||
public class FieldObject2d {
|
||||
/**
|
||||
* Package-local constructor.
|
||||
*
|
||||
* @param name name
|
||||
*/
|
||||
FieldObject2d(String name) {
|
||||
m_name = name;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the pose from a Pose object.
|
||||
*
|
||||
* @param pose 2D pose
|
||||
*/
|
||||
public synchronized void setPose(Pose2d pose) {
|
||||
setPoses(pose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the pose from x, y, and rotation.
|
||||
*
|
||||
* @param xMeters X location, in meters
|
||||
* @param yMeters Y location, in meters
|
||||
* @param rotation rotation
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) {
|
||||
setPose(new Pose2d(xMeters, yMeters, rotation));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the pose.
|
||||
*
|
||||
* @return 2D pose
|
||||
*/
|
||||
public synchronized Pose2d getPose() {
|
||||
updateFromEntry();
|
||||
if (m_poses.isEmpty()) {
|
||||
return new Pose2d();
|
||||
}
|
||||
return m_poses.get(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set multiple poses from an list of Pose objects.
|
||||
* The total number of poses is limited to 85.
|
||||
*
|
||||
* @param poses list of 2D poses
|
||||
*/
|
||||
public synchronized void setPoses(List<Pose2d> poses) {
|
||||
m_poses.clear();
|
||||
for (Pose2d pose : poses) {
|
||||
m_poses.add(pose);
|
||||
}
|
||||
updateEntry();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set multiple poses from an list of Pose objects.
|
||||
* The total number of poses is limited to 85.
|
||||
*
|
||||
* @param poses list of 2D poses
|
||||
*/
|
||||
public synchronized void setPoses(Pose2d... poses) {
|
||||
m_poses.clear();
|
||||
for (Pose2d pose : poses) {
|
||||
m_poses.add(pose);
|
||||
}
|
||||
updateEntry();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get multiple poses.
|
||||
*
|
||||
* @return list of 2D poses
|
||||
*/
|
||||
public synchronized List<Pose2d> getPoses() {
|
||||
updateFromEntry();
|
||||
return new ArrayList<Pose2d>(m_poses);
|
||||
}
|
||||
|
||||
void updateEntry() {
|
||||
updateEntry(false);
|
||||
}
|
||||
|
||||
synchronized void updateEntry(boolean setDefault) {
|
||||
if (m_entry == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
double[] arr = new double[m_poses.size() * 3];
|
||||
int ndx = 0;
|
||||
for (Pose2d pose : m_poses) {
|
||||
Translation2d translation = pose.getTranslation();
|
||||
arr[ndx + 0] = translation.getX();
|
||||
arr[ndx + 1] = translation.getY();
|
||||
arr[ndx + 2] = pose.getRotation().getDegrees();
|
||||
ndx += 3;
|
||||
}
|
||||
|
||||
if (setDefault) {
|
||||
m_entry.setDefaultDoubleArray(arr);
|
||||
} else {
|
||||
m_entry.setDoubleArray(arr);
|
||||
}
|
||||
}
|
||||
|
||||
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
|
||||
private synchronized void updateFromEntry() {
|
||||
if (m_entry == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
double[] arr = m_entry.getDoubleArray((double[]) null);
|
||||
if (arr == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
if ((arr.length % 3) != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_poses.clear();
|
||||
for (int i = 0; i < arr.length; i += 3) {
|
||||
m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
|
||||
}
|
||||
}
|
||||
|
||||
String m_name;
|
||||
NetworkTableEntry m_entry;
|
||||
private List<Pose2d> m_poses;
|
||||
}
|
||||
Reference in New Issue
Block a user