mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[examples] Add Computer Vision Pose Estimation and Latency Compensation Example (#4901)
This PR updates the existing differentialdriveposeestimator example to include computer vision pose estimation and latency compensation. The example generates a simulated cameraToTarget transformation, which is then fed into ComputerVisionUtil.objectToRobotPose() to compute the robot's field-relative position exclusively from vision measurements. The vision measurements are applied through DifferentialDrivePoseEstimator.addVisionMeasurement(). The updated example constructs an AprilTagFieldLayout from JSON. This requires a deploy directory, something which isn't currently supported in wpilibjExamples and wpilibcExamples.
This commit is contained in:
CarloWoolsey
@@ -4,10 +4,31 @@
|
||||
|
||||
#include "Drivetrain.h"
|
||||
|
||||
#include <frc/Timer.h>
|
||||
|
||||
#include "ExampleGlobalMeasurementSensor.h"
|
||||
|
||||
Drivetrain::Drivetrain() {
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_rightGroup.SetInverted(true);
|
||||
|
||||
m_gyro.Reset();
|
||||
|
||||
// Set the distance per pulse for the drive encoders. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
(2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value());
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
(2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value());
|
||||
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
|
||||
frc::SmartDashboard::PutData("FieldSim", &m_fieldSim);
|
||||
frc::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
|
||||
}
|
||||
|
||||
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
|
||||
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
|
||||
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
|
||||
@@ -25,16 +46,86 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
|
||||
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
|
||||
}
|
||||
|
||||
void Drivetrain::PublishCameraToObject(
|
||||
frc::Pose3d objectInField, frc::Transform3d robotToCamera,
|
||||
nt::DoubleArrayEntry& cameraToObjectEntry,
|
||||
frc::sim::DifferentialDrivetrainSim drivetrainSimulator) {
|
||||
frc::Pose3d robotInField{drivetrainSimulator.GetPose()};
|
||||
frc::Pose3d cameraInField = robotInField + robotToCamera;
|
||||
frc::Transform3d cameraToObject{cameraInField, objectInField};
|
||||
|
||||
// Publishes double array with Translation3D elements {x, y, z} and Rotation3D
|
||||
// elements {w, x, y, z} which describe the cameraToObject transformation.
|
||||
std::array<double, 7> val{cameraToObject.X().value(),
|
||||
cameraToObject.Y().value(),
|
||||
cameraToObject.Z().value(),
|
||||
cameraToObject.Rotation().GetQuaternion().W(),
|
||||
cameraToObject.Rotation().GetQuaternion().X(),
|
||||
cameraToObject.Rotation().GetQuaternion().Y(),
|
||||
cameraToObject.Rotation().GetQuaternion().Z()};
|
||||
cameraToObjectEntry.Set(val);
|
||||
}
|
||||
|
||||
frc::Pose3d Drivetrain::ObjectToRobotPose(
|
||||
frc::Pose3d objectInField, frc::Transform3d robotToCamera,
|
||||
nt::DoubleArrayEntry& cameraToObjectEntry) {
|
||||
std::vector<double> val{cameraToObjectEntry.Get()};
|
||||
|
||||
// Reconstruct cameraToObject Transform3D from networktables.
|
||||
frc::Translation3d translation{units::meter_t{val[0]}, units::meter_t{val[1]},
|
||||
units::meter_t{val[2]}};
|
||||
frc::Rotation3d rotation{frc::Quaternion{val[3], val[4], val[5], val[6]}};
|
||||
frc::Transform3d cameraToObject{translation, rotation};
|
||||
|
||||
return frc::ObjectToRobotPose(objectInField, cameraToObject, robotToCamera);
|
||||
}
|
||||
|
||||
void Drivetrain::UpdateOdometry() {
|
||||
m_poseEstimator.Update(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()});
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an
|
||||
// example -- on a real robot, this must be calculated based either on latency
|
||||
// or timestamps.
|
||||
m_poseEstimator.AddVisionMeasurement(
|
||||
ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
|
||||
m_poseEstimator.GetEstimatedPosition()),
|
||||
frc::Timer::GetFPGATimestamp() - 0.3_s);
|
||||
// Publish cameraToObject transformation to networktables --this would
|
||||
// normally be handled by the computer vision solution.
|
||||
PublishCameraToObject(m_objectInField, m_robotToCamera,
|
||||
m_cameraToObjectEntryRef, m_drivetrainSimulator);
|
||||
|
||||
// Compute the robot's field-relative position exclusively from vision
|
||||
// measurements.
|
||||
frc::Pose3d visionMeasurement3d = ObjectToRobotPose(
|
||||
m_objectInField, m_robotToCamera, m_cameraToObjectEntryRef);
|
||||
|
||||
// Convert robot's pose from Pose3d to Pose2d needed to apply vision
|
||||
// measurements.
|
||||
frc::Pose2d visionMeasurement2d = visionMeasurement3d.ToPose2d();
|
||||
|
||||
// Apply vision measurements. For simulation purposes only, we don't input a
|
||||
// latency delay -- on a real robot, this must be calculated based either on
|
||||
// known latency or timestamps.
|
||||
m_poseEstimator.AddVisionMeasurement(visionMeasurement2d,
|
||||
frc::Timer::GetFPGATimestamp());
|
||||
}
|
||||
|
||||
void Drivetrain::SimulationPeriodic() {
|
||||
// To update our simulation, we set motor voltage inputs, update the
|
||||
// simulation, and write the simulated positions and velocities to our
|
||||
// simulated encoder and gyro.
|
||||
m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} *
|
||||
frc::RobotController::GetInputVoltage(),
|
||||
units::volt_t{m_rightGroup.Get()} *
|
||||
frc::RobotController::GetInputVoltage());
|
||||
m_drivetrainSimulator.Update(20_ms);
|
||||
|
||||
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
|
||||
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
|
||||
m_rightEncoderSim.SetDistance(
|
||||
m_drivetrainSimulator.GetRightPosition().value());
|
||||
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
|
||||
m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
|
||||
}
|
||||
|
||||
void Drivetrain::Periodic() {
|
||||
UpdateOdometry();
|
||||
m_fieldSim.SetRobotPose(m_drivetrainSimulator.GetPose());
|
||||
m_fieldApproximation.SetRobotPose(m_poseEstimator.GetEstimatedPosition());
|
||||
}
|
||||
|
||||
@@ -15,6 +15,8 @@ class Robot : public frc::TimedRobot {
|
||||
m_drive.UpdateOdometry();
|
||||
}
|
||||
|
||||
void RobotPeriodic() override { m_drive.Periodic(); }
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
@@ -31,6 +33,8 @@ class Robot : public frc::TimedRobot {
|
||||
m_drive.Drive(xSpeed, rot);
|
||||
}
|
||||
|
||||
void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
|
||||
|
||||
private:
|
||||
frc::XboxController m_controller{0};
|
||||
|
||||
|
||||
@@ -7,13 +7,30 @@
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/AnalogGyro.h>
|
||||
#include <frc/ComputerVisionUtil.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/estimator/DifferentialDrivePoseEstimator.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
#include <frc/geometry/Quaternion.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
#include <frc/kinematics/DifferentialDriveKinematics.h>
|
||||
#include <frc/motorcontrol/MotorControllerGroup.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/AnalogGyroSim.h>
|
||||
#include <frc/simulation/DifferentialDrivetrainSim.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <frc/smartdashboard/Field2d.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc/system/plant/LinearSystemId.h>
|
||||
#include <networktables/DoubleArrayTopic.h>
|
||||
#include <networktables/NetworkTableInstance.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
@@ -24,40 +41,100 @@
|
||||
*/
|
||||
class Drivetrain {
|
||||
public:
|
||||
Drivetrain() {
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_rightGroup.SetInverted(true);
|
||||
|
||||
m_gyro.Reset();
|
||||
// Set the distance per pulse for the drive encoders. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
2 * std::numbers::pi * kWheelRadius.value() / kEncoderResolution);
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
2 * std::numbers::pi * kWheelRadius.value() / kEncoderResolution);
|
||||
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
Drivetrain();
|
||||
|
||||
static constexpr units::meters_per_second_t kMaxSpeed =
|
||||
3.0_mps; // 3 meters per second
|
||||
static constexpr units::radians_per_second_t kMaxAngularSpeed{
|
||||
std::numbers::pi}; // 1/2 rotation per second
|
||||
|
||||
/**
|
||||
* Sets the desired wheel speeds.
|
||||
*
|
||||
* @param speeds The desired wheel speeds.
|
||||
*/
|
||||
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
|
||||
|
||||
/** Drives the robot with the given linear velocity and angular velocity.
|
||||
*
|
||||
* @param xSpeed Linear velocity.
|
||||
* @param rot Angular Velocity.
|
||||
*/
|
||||
void Drive(units::meters_per_second_t xSpeed,
|
||||
units::radians_per_second_t rot);
|
||||
|
||||
/**
|
||||
* Updates the field-relative position.
|
||||
*/
|
||||
void UpdateOdometry();
|
||||
|
||||
/**
|
||||
* This function is called periodically during simulation. */
|
||||
void SimulationPeriodic();
|
||||
|
||||
/** This function is called periodically, regardless of mode. */
|
||||
void Periodic();
|
||||
|
||||
/**
|
||||
* Computes and publishes to a networktables topic the transformation from
|
||||
* the camera's pose to the object's pose. This function exists solely for the
|
||||
* purposes of simulation, and this would normally be handled by computer
|
||||
* vision.
|
||||
*
|
||||
* <p>The object could be a target or a fiducial marker.
|
||||
*
|
||||
* @param objectInField The object's field-relative position.
|
||||
* @param robotToCamera The transformation from the robot's pose to the
|
||||
* camera's pose.
|
||||
* @param cameraToObjectEntry The networktables entry publishing and querying
|
||||
* example computer vision measurements.
|
||||
* @param drivetrainSimulation A DifferentialDrivetrainSim modeling the
|
||||
* robot's drivetrain.
|
||||
*/
|
||||
void PublishCameraToObject(
|
||||
frc::Pose3d objectInField, frc::Transform3d robotToCamera,
|
||||
nt::DoubleArrayEntry& cameraToObjectEntry,
|
||||
frc::sim::DifferentialDrivetrainSim drivetrainSimulator);
|
||||
|
||||
/**
|
||||
* Queries the camera-to-object transformation from networktables to compute
|
||||
* the robot's field-relative pose from vision measurements.
|
||||
*
|
||||
* <p>The object could be a target or a fiducial marker.
|
||||
*
|
||||
* @param objectInField The object's field-relative position.
|
||||
* @param robotToCamera The transformation from the robot's pose to the
|
||||
* camera's pose.
|
||||
* @param cameraToObjectEntry The networktables entry publishing and querying
|
||||
* example computer vision measurements.
|
||||
*/
|
||||
frc::Pose3d ObjectToRobotPose(frc::Pose3d objectInField,
|
||||
frc::Transform3d robotToCamera,
|
||||
nt::DoubleArrayEntry& cameraToObjectEntry);
|
||||
|
||||
private:
|
||||
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
|
||||
static constexpr units::meter_t kWheelRadius = 0.0508_m;
|
||||
static constexpr int kEncoderResolution = 4096;
|
||||
|
||||
static constexpr std::array<double, 7> kDefaultVal{0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0};
|
||||
|
||||
frc::Transform3d m_robotToCamera{
|
||||
frc::Translation3d{1_m, 1_m, 1_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}}};
|
||||
|
||||
nt::NetworkTableInstance m_inst{nt::NetworkTableInstance::GetDefault()};
|
||||
nt::DoubleArrayTopic m_cameraToObjectTopic{
|
||||
m_inst.GetDoubleArrayTopic("m_cameraToObjectTopic")};
|
||||
nt::DoubleArrayEntry m_cameraToObjectEntry =
|
||||
m_cameraToObjectTopic.GetEntry(kDefaultVal);
|
||||
nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
|
||||
|
||||
frc::AprilTagFieldLayout m_aprilTagFieldLayout{
|
||||
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2022RapidReact)};
|
||||
frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
|
||||
|
||||
frc::PWMSparkMax m_leftLeader{1};
|
||||
frc::PWMSparkMax m_leftFollower{2};
|
||||
frc::PWMSparkMax m_rightLeader{3};
|
||||
@@ -90,4 +167,16 @@ class Drivetrain {
|
||||
// Gains are for example purposes only - must be determined for your own
|
||||
// robot!
|
||||
frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
|
||||
|
||||
// Simulation classes
|
||||
frc::sim::AnalogGyroSim m_gyroSim{m_gyro};
|
||||
frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
|
||||
frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
|
||||
frc::Field2d m_fieldSim;
|
||||
frc::Field2d m_fieldApproximation;
|
||||
frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
|
||||
frc::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
|
||||
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
|
||||
m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
|
||||
};
|
||||
|
||||
@@ -4,21 +4,42 @@
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.ComputerVisionUtil;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Quaternion;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.system.LinearSystem;
|
||||
import edu.wpi.first.math.system.plant.DCMotor;
|
||||
import edu.wpi.first.math.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.networktables.DoubleArrayEntry;
|
||||
import edu.wpi.first.networktables.DoubleArrayTopic;
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
|
||||
import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
|
||||
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
|
||||
import edu.wpi.first.wpilibj.simulation.EncoderSim;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import java.io.IOException;
|
||||
|
||||
/** Represents a differential drive style drivetrain. */
|
||||
public class Drivetrain {
|
||||
@@ -50,6 +71,18 @@ public class Drivetrain {
|
||||
private final DifferentialDriveKinematics m_kinematics =
|
||||
new DifferentialDriveKinematics(kTrackWidth);
|
||||
|
||||
private final Pose3d m_objectInField;
|
||||
|
||||
private final Transform3d m_robotToCamera =
|
||||
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, Math.PI / 2));
|
||||
|
||||
private final DoubleArrayEntry m_cameraToObjectEntry;
|
||||
|
||||
private final double[] m_defaultVal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
|
||||
private final Field2d m_fieldSim = new Field2d();
|
||||
private final Field2d m_fieldApproximation = new Field2d();
|
||||
|
||||
/* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
|
||||
numbers used below are robot specific, and should be tuned. */
|
||||
private final DifferentialDrivePoseEstimator m_poseEstimator =
|
||||
@@ -65,11 +98,21 @@ public class Drivetrain {
|
||||
// Gains are for example purposes only - must be determined for your own robot!
|
||||
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
|
||||
|
||||
// Simulation classes
|
||||
private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
|
||||
private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
|
||||
private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
|
||||
private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
|
||||
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
|
||||
private final DifferentialDrivetrainSim m_drivetrainSimulator =
|
||||
new DifferentialDrivetrainSim(
|
||||
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
|
||||
|
||||
/**
|
||||
* Constructs a differential drive object. Sets the encoder distance per pulse and resets the
|
||||
* gyro.
|
||||
*/
|
||||
public Drivetrain() {
|
||||
public Drivetrain(DoubleArrayTopic cameraToObjectTopic) {
|
||||
m_gyro.reset();
|
||||
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
@@ -85,6 +128,21 @@ public class Drivetrain {
|
||||
|
||||
m_leftEncoder.reset();
|
||||
m_rightEncoder.reset();
|
||||
|
||||
m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal);
|
||||
|
||||
try {
|
||||
m_objectInField =
|
||||
AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile)
|
||||
.getTagPose(0)
|
||||
.get();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
throw new RuntimeException();
|
||||
}
|
||||
|
||||
SmartDashboard.putData("Field", m_fieldSim);
|
||||
SmartDashboard.putData("FieldEstimation", m_fieldApproximation);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -115,16 +173,109 @@ public class Drivetrain {
|
||||
setSpeeds(wheelSpeeds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes and publishes to a network tables topic the transformation from the camera's pose to
|
||||
* the object's pose. This function exists solely for the purposes of simulation, and this would
|
||||
* normally be handled by computer vision.
|
||||
*
|
||||
* <p>The object could be a target or a fiducial marker.
|
||||
*
|
||||
* @param objectInField The object's field-relative position.
|
||||
* @param robotToCamera The transformation from the robot's pose to the camera's pose.
|
||||
* @param cameraToObjectEntry The networktables entry publishing and querying example computer
|
||||
* vision measurements.
|
||||
*/
|
||||
public void publishCameraToObject(
|
||||
Pose3d objectInField,
|
||||
Transform3d robotToCamera,
|
||||
DoubleArrayEntry cameraToObjectEntry,
|
||||
DifferentialDrivetrainSim drivetrainSimulator) {
|
||||
Pose3d robotInField = new Pose3d(drivetrainSimulator.getPose());
|
||||
Pose3d cameraInField = robotInField.plus(robotToCamera);
|
||||
Transform3d cameraToObject = new Transform3d(cameraInField, objectInField);
|
||||
|
||||
// Publishes double array with Translation3D elements {x, y, z} and Rotation3D elements {w, x,
|
||||
// y, z} which describe
|
||||
// the cameraToObject transformation.
|
||||
double[] val = {
|
||||
cameraToObject.getX(),
|
||||
cameraToObject.getY(),
|
||||
cameraToObject.getZ(),
|
||||
cameraToObject.getRotation().getQuaternion().getW(),
|
||||
cameraToObject.getRotation().getQuaternion().getX(),
|
||||
cameraToObject.getRotation().getQuaternion().getY(),
|
||||
cameraToObject.getRotation().getQuaternion().getZ()
|
||||
};
|
||||
cameraToObjectEntry.set(val);
|
||||
}
|
||||
|
||||
/**
|
||||
* Queries the camera-to-object transformation from networktables to compute the robot's
|
||||
* field-relative pose from vision measurements.
|
||||
*
|
||||
* <p>The object could be a target or a fiducial marker.
|
||||
*
|
||||
* @param objectInField The object's field-relative pose.
|
||||
* @param robotToCamera The transformation from the robot's pose to the camera's pose.
|
||||
* @param cameraToObjectEntry The networktables entry publishing and querying example computer
|
||||
* vision measurements.
|
||||
*/
|
||||
public Pose3d objectToRobotPose(
|
||||
Pose3d objectInField, Transform3d robotToCamera, DoubleArrayEntry cameraToObjectEntry) {
|
||||
double[] val = cameraToObjectEntry.get();
|
||||
|
||||
// Reconstruct cameraToObject Transform3D from networktables.
|
||||
Translation3d translation = new Translation3d(val[0], val[1], val[2]);
|
||||
Rotation3d rotation = new Rotation3d(new Quaternion(val[3], val[4], val[5], val[6]));
|
||||
Transform3d cameraToObject = new Transform3d(translation, rotation);
|
||||
|
||||
return ComputerVisionUtil.objectToRobotPose(objectInField, cameraToObject, robotToCamera);
|
||||
}
|
||||
|
||||
/** Updates the field-relative position. */
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update(
|
||||
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
m_poseEstimator.addVisionMeasurement(
|
||||
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
|
||||
m_poseEstimator.getEstimatedPosition()),
|
||||
Timer.getFPGATimestamp() - 0.3);
|
||||
// Publish cameraToObject transformation to networktables --this would normally be handled by
|
||||
// the
|
||||
// computer vision solution.
|
||||
publishCameraToObject(
|
||||
m_objectInField, m_robotToCamera, m_cameraToObjectEntry, m_drivetrainSimulator);
|
||||
|
||||
// Compute the robot's field-relative position exclusively from vision measurements.
|
||||
Pose3d visionMeasurement3d =
|
||||
objectToRobotPose(m_objectInField, m_robotToCamera, m_cameraToObjectEntry);
|
||||
|
||||
// Convert robot pose from Pose3d to Pose2d needed to apply vision measurements.
|
||||
Pose2d visionMeasurement2d = visionMeasurement3d.toPose2d();
|
||||
|
||||
// Apply vision measurements. For simulation purposes only, we don't input a latency delay -- on
|
||||
// a real robot, this must be calculated based either on known latency or timestamps.
|
||||
m_poseEstimator.addVisionMeasurement(visionMeasurement2d, Timer.getFPGATimestamp());
|
||||
}
|
||||
|
||||
/** This function is called periodically during simulation. */
|
||||
public void simulationPeriodic() {
|
||||
// To update our simulation, we set motor voltage inputs, update the
|
||||
// simulation, and write the simulated positions and velocities to our
|
||||
// simulated encoder and gyro.
|
||||
m_drivetrainSimulator.setInputs(
|
||||
m_leftGroup.get() * RobotController.getInputVoltage(),
|
||||
m_rightGroup.get() * RobotController.getInputVoltage());
|
||||
m_drivetrainSimulator.update(0.02);
|
||||
|
||||
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
|
||||
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
|
||||
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
|
||||
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
|
||||
m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
|
||||
}
|
||||
|
||||
/** This function is called periodically, no matter the mode. */
|
||||
public void periodic() {
|
||||
updateOdometry();
|
||||
m_fieldSim.setRobotPose(m_drivetrainSimulator.getPose());
|
||||
m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,12 +5,18 @@
|
||||
package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.math.filter.SlewRateLimiter;
|
||||
import edu.wpi.first.networktables.DoubleArrayTopic;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private final NetworkTableInstance m_inst = NetworkTableInstance.getDefault();
|
||||
private final DoubleArrayTopic m_doubleArrayTopic =
|
||||
m_inst.getDoubleArrayTopic("m_doubleArrayTopic");
|
||||
|
||||
private final XboxController m_controller = new XboxController(0);
|
||||
private final Drivetrain m_drive = new Drivetrain();
|
||||
private final Drivetrain m_drive = new Drivetrain(m_doubleArrayTopic);
|
||||
|
||||
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
|
||||
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
|
||||
@@ -22,6 +28,16 @@ public class Robot extends TimedRobot {
|
||||
m_drive.updateOdometry();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void simulationPeriodic() {
|
||||
m_drive.simulationPeriodic();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
m_drive.periodic();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
|
||||
Reference in New Issue
Block a user