us.ihmc.scs2.session.log.LogSession Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of scs2-session-logger
SCS2 Session Logger
The newest version!
package us.ihmc.scs2.session.log;

import us.ihmc.commons.Conversions;
import us.ihmc.graphicsDescription.conversion.YoGraphicConversionTools;
import us.ihmc.log.LogTools;
import us.ihmc.robotDataLogger.handshake.YoVariableHandshakeParser;
import us.ihmc.robotDataLogger.logger.LogPropertiesReader;
import us.ihmc.scs2.definition.robot.RobotDefinition;
import us.ihmc.scs2.definition.robot.RobotStateDefinition;
import us.ihmc.scs2.definition.terrain.TerrainObjectDefinition;
import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
import us.ihmc.scs2.session.Session;
import us.ihmc.scs2.session.SessionMode;
import us.ihmc.scs2.session.tools.RobotDataLogTools;
import us.ihmc.scs2.session.tools.RobotModelLoader;
import us.ihmc.scs2.sharedMemory.interfaces.YoBufferPropertiesReadOnly;
import us.ihmc.scs2.simulation.TimeConsumer;
import us.ihmc.scs2.simulation.robot.Robot;

import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.Objects;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.stream.Collectors;

public class LogSession extends Session
{
   private final String sessionName;
   private final List robots = new ArrayList<>();
   private final List robotDefinitions = new ArrayList<>();
   private final List yoGraphicDefinitions = new ArrayList<>();
   private final Runnable robotStateUpdater;

   private final File logDirectory;
   private final LogDataReader logDataReader;
   private final LogPropertiesReader logProperties;

   /**
    * This is used to jump to a specific position in the log when the user drags the slider.
    * 
    * It is thread-safe.
    * 
    */
   private final AtomicInteger logPositionRequest = new AtomicInteger(-1);

   private final List afterReadCallbacks = new ArrayList<>();

   public LogSession(File logDirectory, ProgressConsumer progressConsumer) throws IOException
   {
      this.logDirectory = logDirectory;
      try
      {
         logDataReader = new LogDataReader(logDirectory, progressConsumer);
         LogTools.info("Created data reader.");
      }
      catch (IOException e)
      {
         throw new RuntimeException(e);
      }
      logProperties = logDataReader.getLogProperties();

      YoVariableHandshakeParser parser = logDataReader.getParser();
      rootRegistry.addChild(logDataReader.getYoRegistry());
      rootRegistry.addChild(parser.getRootRegistry());
      yoGraphicDefinitions.add(new YoGraphicGroupDefinition("SCS1 YoGraphics", YoGraphicConversionTools.toYoGraphicDefinitions(parser.getSCS1YoGraphics())));
      if (parser.getSCS2YoGraphics() != null)
         yoGraphicDefinitions.addAll(parser.getSCS2YoGraphics());

      sessionName = logProperties.getNameAsString();

      RobotDefinition robotDefinition = RobotDataLogTools.loadRobotDefinition(logDirectory, logProperties);

      if (robotDefinition != null)
      {
         robotDefinitions.add(robotDefinition);
         Robot robot = new Robot(robotDefinition, getInertialFrame());
         robots.add(robot);
         robotStateUpdater = RobotModelLoader.setupRobotUpdater(robot, parser, rootRegistry);
      }
      else
      {
         robotStateUpdater = null;
      }

      setDesiredBufferPublishPeriod(Conversions.secondsToNanoseconds(1.0 / 30.0));
      setSessionDTSeconds(parser.getDt());
      setSessionMode(SessionMode.PAUSE);
   }

   public void submitLogPositionRequest(int logPosition)
   {
      logPositionRequest.set(logPosition);
   }

   @Override
   protected void initializeSession()
   {
      // We read the very first frame of the log.
      logDataReader.read();

      if (robotStateUpdater != null)
         robotStateUpdater.run();
   }

   @Override
   protected void initializeRunTick()
   {
      if (firstRunTick)
      {
         // TODO Can probably be a little smarter here, sometimes we don't need to reset the equation manager.
         equationManager.reset();

         YoBufferPropertiesReadOnly properties = sharedBuffer.getProperties();

         if (properties.getCurrentIndex() != properties.getOutPoint())
            sharedBuffer.setInPoint(properties.getCurrentIndex());
         else if (!firstLogPositionRequest) // That means the user has scrubbed through the data.
            sharedBuffer.setInPoint(properties.getCurrentIndex());
         sharedBuffer.incrementBufferIndex(true);
         // Sync the log position index (logDataReader.index) the current YoVariable (logDataReader.currentRecordTick()) value.
         // Without that, scrubbing through a chart and then resuming log reading will start from an arbitrary position in the log file (corresponding to where we last stop reading the log file).
         logDataReader.seek(logDataReader.getCurrentLogPosition());
         nextRunBufferRecordTickCounter = 0;
         firstRunTick = false;
      }
      else if (nextRunBufferRecordTickCounter <= 0)
      {
         sharedBuffer.incrementBufferIndex(true);
         sharedBuffer.processLinkedPushRequests(false);
      }

      // Push from the linked registries are unnecessary when reading a log file.
   }

   @Override
   protected double doSpecificRunTick()
   {
      boolean endOfLog = logDataReader.read();
      if (endOfLog)
         setSessionMode(SessionMode.PAUSE);

      if (robotStateUpdater != null)
         robotStateUpdater.run();

      double currentTime = logDataReader.getCurrentRobotTime();

      for (int i = 0; i < afterReadCallbacks.size(); i++)
         afterReadCallbacks.get(i).accept(currentTime);

      return currentTime;
   }

   private boolean firstLogPositionRequest = true;

   @Override
   public void pauseTick()
   {
      if (firstPauseTick)
         firstLogPositionRequest = true;

      int logPosition = logPositionRequest.getAndSet(-1);

      if (logPosition == -1)
      {
         super.pauseTick();
      }
      else
      {// Handles when the user is scrubbing through the log using the log slider.
         processBufferRequests(false);

         logDataReader.seek(logPosition);
         logDataReader.read();

         if (robotStateUpdater != null)
            robotStateUpdater.run();

         if (firstLogPositionRequest)
         { // We increment only once when starting to scrub through the data to not write on the last data point.
            sharedBuffer.incrementBufferIndex(true);
            firstLogPositionRequest = false;
         }
         sharedBuffer.writeBuffer();
         sharedBuffer.prepareLinkedBuffersForPull();
         publishBufferProperties(sharedBuffer.getProperties());
      }
   }

   /**
    * Adds a callback to be executed after reading each line of the log file.
    * 
    * This can be used to register some post-processing on the log data.
    * 
    *
    * @param callback the callback to add.
    */
   public void addAfterReadCallback(TimeConsumer callback)
   {
      afterReadCallbacks.add(Objects.requireNonNull(callback, "The callback cannot be null."));
   }

   /**
    * Removes a callback that was previously added.
    *
    * @param callback the callback to remove.
    * @return {@code true} if the callback was removed, {@code false} if it was not found.
    */
   public boolean removeAfterReadCallback(TimeConsumer callback)
   {
      return afterReadCallbacks.remove(callback);
   }

   @Override
   public String getSessionName()
   {
      return sessionName;
   }

   @Override
   public List getRobotDefinitions()
   {
      return robotDefinitions;
   }

   @Override
   public List getTerrainObjectDefinitions()
   {
      return Collections.emptyList();
   }

   @Override
   public List getYoGraphicDefinitions()
   {
      return yoGraphicDefinitions;
   }

   public List getRobots()
   {
      return robots;
   }

   @Override
   public List getCurrentRobotStateDefinitions(boolean initialState)
   {
      return robots.stream().map(Robot::getCurrentRobotStateDefinition).collect(Collectors.toList());
   }

   public File getLogDirectory()
   {
      return logDirectory;
   }

   public LogDataReader getLogDataReader()
   {
      return logDataReader;
   }

   public LogPropertiesReader getLogProperties()
   {
      return logProperties;
   }
}