VehPosePlayer.cc

Go to the documentation of this file.

#include <utils/Linear.h>

#include "VehPosePlayer.h"
#include "VehPoseSource.h"

bool VehPosePlayer::open(utils::ConfigFile& params,
                         utils::SymbolTable* globals)
{
  // setup the player manager
  // the player manager takes care of all of the interactions between the
  // data file and the time source
  if (!_mgr.open("VehPose.rad", params, globals))
    return false;

  // get the actual canned data reader from the manager
  _player = _mgr.getPlayer();

  // clear the input area
  memset(&_input_area, 0, sizeof(_input_area));

  // check versions
  int major_version = _mgr.getPlayer()->getHeader().
    getInt("int DataFormat.version_major", 1);
  int minor_version = _mgr.getPlayer()->getHeader().
    getInt("int DataFormat.version_minor", 1);
  // note, you don't have to simply reject other version, you can
  // try and adapt here
  if (major_version != 1 && minor_version != 0) {
    printf("VehPosePlayer::init:  Cannot read version %d.%d\n",
           major_version, minor_version);
    return false;
  }

  // Tell the player to expect pose.  We don't store any element references
  // because we know these are all simple, flat structures which don't have
  // to be cleaned up
  _player->expect("x", "double", &_input_area.x);
  _player->expect("y", "double", &_input_area.y);
  _player->expect("z", "float", &_input_area.z);;
  _player->expect("ori", "[float : 4]", &_input_area.ori);

  // Ready the player for action
  return _player->setup();
}

bool VehPosePlayer::advance()
{
  // advance the file pointer, if necessary, 
  // and cache the last read time for later
  // the player manager takes care of all the necessary interactions with
  // time, i.e., does reading advance time, or do we observe time to see
  // where to read.
  return _mgr.next(_play_time);
}

void VehPosePlayer::set_pose(VehPose& pose)
{
  pose.pos = utils::Vec3d(_input_area.x, _input_area.y, _input_area.z);
  pose.ori = utils::Rotation(_input_area.ori[0], _input_area.ori[1],
                             _input_area.ori[2], _input_area.ori[3]);  
}

bool VehPosePlayer::getCurPose(utils::Time& time, VehPose& pose)
{
  set_pose(pose);
  
  // and set time from cached value
  time = _play_time;

  return true;
}

bool VehPosePlayer::nextPose(utils::Time& time, VehPose& pose, bool blocking)
{
  if (blocking || (!blocking && _mgr.poll())) {
    if (!advance())
      return false;
  }
  return getCurPose(time, pose);
}

bool VehPosePlayer::getPose(utils::Time time, VehPose& pose)
{
  // get pose before target
  utils::Time prev_time = time;
  if (!_player->get(prev_time)) {
    return false;
  }
  VehPose prev_pose;
  set_pose(prev_pose);

  // if we have an exact match, return
  if (time == prev_time) {
    pose = prev_pose;
    return true;
  }

  // get pose after target
  utils::Time next_time;
  if (!_player->next() || !_player->process(next_time)) {
    return false;
  }

  // if we have an exact match, return
  VehPose next_pose;
  set_pose(next_pose);
  if (time == next_time) {
    pose = next_pose;
    return true;
  }

  // if we do not have two different elements, we cannot extrapolate
  if (prev_time == next_time) {
    time = utils::Time();
    return false;
  }

  // attempte to interpolate between the two poses
  double period = (next_time-prev_time).getValue();
  double elapsed = (time-prev_time).getValue();
  double t = elapsed/period;

  VehPoseSource::interpolate(prev_pose, next_pose, t, pose);
  return true;
}  

---