Skip to content

Generate Javadoc for sensor data context. #793

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: develop
Choose a base branch
from
Open

Generate Javadoc for sensor data context. #793

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
127 changes: 117 additions & 10 deletions ...processing/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/SensorDataContext.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,49 +17,84 @@
import us.ihmc.sensorProcessing.outputData.ImuData;
import us.ihmc.sensorProcessing.outputData.LowLevelState;

/**
* A class for managing sensor data from joints, IMUs, and force sensors in a robotic system.
* It provides methods to register sensors, retrieve their measurements, and manage the data context.
* Implements {@link Settable} to allow copying data from another {@code SensorDataContext}.
*/
public class SensorDataContext implements Settable<SensorDataContext>
{
/** List of registered joint names. */
private final List<String> jointNames = new ArrayList<>();
/** List of joint measurements. */
private final List<LowLevelState> jointMeasurements = new ArrayList<>();
/** Set of joint name hash codes for quick lookup. */
private final transient TLongSet jointSet = new TLongHashSet();
/** Map of joint names to their measurements. */
private final transient Map<String, LowLevelState> jointMeasurementMap = new HashMap<>();

/** List of registered IMU names. */
private final List<String> imuNames = new ArrayList<>();
/** List of IMU measurements. */
private final List<ImuData> imuMeasurements = new ArrayList<>();
/** Set of IMU name hash codes for quick lookup. */
private final transient TLongSet imuSet = new TLongHashSet();
/** Map of IMU names to their measurements. */
private final transient Map<String, ImuData> imuMeasurementMap = new HashMap<>();

/** List of registered force sensor names. */
private final List<String> forceSensorNames = new ArrayList<>();
/** List of force sensor measurements. */
private final List<DMatrixRMaj> forceSensorMeasurements = new ArrayList<>();
/** Set of force sensor name hash codes for quick lookup. */
private final transient TLongSet forceSensorSet = new TLongHashSet();
/** Map of force sensor names to their measurements. */
private final transient Map<String, DMatrixRMaj> forceSensorMeasurementMap = new HashMap<>();

/**
* Constructs an empty {@code SensorDataContext}.
*/
public SensorDataContext()
{
}

/**
* Constructs a {@code SensorDataContext} and registers all joints, IMUs, and force sensors from the provided robot model.
*
* @param fullRobotModel The robot model containing joint, IMU, and force sensor definitions.
*/
public SensorDataContext(FullRobotModel fullRobotModel)
{
Arrays.asList(fullRobotModel.getOneDoFJoints()).forEach(joint -> registerJoint(joint.getName()));
Arrays.asList(fullRobotModel.getIMUDefinitions()).forEach(imu -> registerImu(imu.getName()));
Arrays.asList(fullRobotModel.getForceSensorDefinitions()).forEach(forceSensor -> registerForceSensor(forceSensor.getSensorName()));
Arrays.asList(fullRobotModel.getForceSensorDefinitions()).forEach(sensor -> registerForceSensor(sensor.getSensorName()));
}

/**
* Constructs a {@code SensorDataContext} and registers the provided joints.
*
* @param joints The list of joints to register.
*/
public SensorDataContext(List<OneDoFJointBasics> joints)
{
joints.forEach(joint -> registerJoint(joint.getName()));
}

/**
* Registers a joint with the given name and returns its associated measurement object.
* Reuses existing measurement data if the joint was previously registered.
*
* @param jointName The name of the joint to register.
* @return The {@link LowLevelState} object for the joint's measurements.
* @throws RuntimeException If a joint with the same name hash is already registered.
*/
public LowLevelState registerJoint(String jointName)
{
// Check if a joint with the same name is already registered.
// It is very unlikely but it could happen that two joint names produce the same hash code.
if (isJointRegistered(jointName))
{
throw new RuntimeException("Joint with same name hash as " + jointName + " is already registered.");
}

// If the joint was ever registered in the past reuse the existing data structure.
LowLevelState lowLevelState = jointMeasurementMap.get(jointName);
if (lowLevelState == null)
{
Expand All @@ -74,16 +109,21 @@ public LowLevelState registerJoint(String jointName)
return lowLevelState;
}

/**
* Registers an IMU with the given name and returns its associated measurement object.
* Reuses existing measurement data if the IMU was previously registered.
*
* @param imuName The name of the IMU to register.
* @return The {@link ImuData} object for the IMU's measurements.
* @throws RuntimeException If an IMU with the same name hash is already registered.
*/
public ImuData registerImu(String imuName)
{
// Check if an IMU with the same name is already registered.
// It is very unlikely but it could happen that two IMU names produce the same hash code.
if (isImuRegistered(imuName))
{
throw new RuntimeException("IMU with same name hash as " + imuName + " is already registered.");
}

// If the IMU was ever registered in the past reuse the existing data structure.
ImuData imuData = imuMeasurementMap.get(imuName);
if (imuData == null)
{
Expand All @@ -98,16 +138,21 @@ public ImuData registerImu(String imuName)
return imuData;
}

/**
* Registers a force sensor with the given name and returns its associated measurement matrix.
* Reuses existing measurement data if the force sensor was previously registered.
*
* @param forceSensorName The name of the force sensor to register.
* @return The {@link DMatrixRMaj} object for the force sensor's measurements (6x1 matrix).
* @throws RuntimeException If a force sensor with the same name hash is already registered.
*/
public DMatrixRMaj registerForceSensor(String forceSensorName)
{
// Check if an force sensor with the same name is already registered.
// It is very unlikely but it could happen that two force sensor names produce the same hash code.
if (isForceSensorRegistered(forceSensorName))
{
throw new RuntimeException("Force Sensor with same name hash as " + forceSensorName + " is already registered.");
}

// If the force sensor was ever registered in the past reuse the existing data structure.
DMatrixRMaj forceSensorData = forceSensorMeasurementMap.get(forceSensorName);
if (forceSensorData == null)
{
Expand All @@ -122,6 +167,13 @@ public DMatrixRMaj registerForceSensor(String forceSensorName)
return forceSensorData;
}

/**
* Retrieves the measurement data for the specified joint.
*
* @param jointName The name of the joint.
* @return The {@link LowLevelState} object containing the joint's measurements.
* @throws RuntimeException If the joint is not registered.
*/
public LowLevelState getMeasuredJointState(String jointName)
{
if (!isJointRegistered(jointName))
Expand All @@ -131,6 +183,13 @@ public LowLevelState getMeasuredJointState(String jointName)
return jointMeasurementMap.get(jointName);
}

/**
* Retrieves the measurement data for the specified IMU.
*
* @param imuName The name of the IMU.
* @return The {@link ImuData} object containing the IMU's measurements.
* @throws RuntimeException If the IMU is not registered.
*/
public ImuData getImuMeasurement(String imuName)
{
if (!isImuRegistered(imuName))
Expand All @@ -140,6 +199,13 @@ public ImuData getImuMeasurement(String imuName)
return imuMeasurementMap.get(imuName);
}

/**
* Retrieves the measurement data for the specified force sensor.
*
* @param forceSensorName The name of the force sensor.
* @return The {@link DMatrixRMaj} object containing the force sensor's measurements.
* @throws RuntimeException If the force sensor is not registered.
*/
public DMatrixRMaj getForceSensorMeasurement(String forceSensorName)
{
if (!isForceSensorRegistered(forceSensorName))
Expand All @@ -149,21 +215,42 @@ public DMatrixRMaj getForceSensorMeasurement(String forceSensorName)
return forceSensorMeasurementMap.get(forceSensorName);
}

/**
* Checks if a joint with the given name is registered.
*
* @param jointName The name of the joint.
* @return {@code true} if the joint is registered, {@code false} otherwise.
*/
public boolean isJointRegistered(String jointName)
{
return jointSet.contains(jointName.hashCode());
}

/**
* Checks if an IMU with the given name is registered.
*
* @param imuName The name of the IMU.
* @return {@code true} if the IMU is registered, {@code false} otherwise.
*/
public boolean isImuRegistered(String imuName)
{
return imuSet.contains(imuName.hashCode());
}

/**
* Checks if a force sensor with the given name is registered.
*
* @param forceSensorName The name of the force sensor.
* @return {@code true} if the force sensor is registered, {@code false} otherwise.
*/
public boolean isForceSensorRegistered(String forceSensorName)
{
return forceSensorSet.contains(forceSensorName.hashCode());
}

/**
* Clears all registered sensors and their measurements.
*/
public void clear()
{
jointNames.clear();
Expand All @@ -179,6 +266,12 @@ public void clear()
forceSensorSet.clear();
}

/**
* Copies the data from another {@code SensorDataContext} into this instance.
* Clears existing data and registers all sensors from the other context.
*
* @param other The {@code SensorDataContext} to copy from.
*/
@Override
public void set(SensorDataContext other)
{
Expand All @@ -197,6 +290,14 @@ public void set(SensorDataContext other)
}
}

/**
* Compares this {@code SensorDataContext} with another object for equality.
* Two {@code SensorDataContext} instances are equal if they have the same registered sensors
* with equal measurements.
*
* @param obj The object to compare with.
* @return {@code true} if the objects are equal, {@code false} otherwise.
*/
@Override
public boolean equals(Object obj)
{
Expand Down Expand Up @@ -249,6 +350,12 @@ else if (obj instanceof SensorDataContext)
}
}

/**
* Returns a string representation of this {@code SensorDataContext}, listing all registered
* sensors and their measurements.
*
* @return A string describing the sensor data.
*/
@Override
public String toString()
{
Expand Down
Loading