ShmemVehPoseSource.cc

Go to the documentation of this file.

#include <stdio.h>
#include <pthread.h>

#include <utils/ConfigFile.h>
#include <TimeSource/TimeSource.h>

#include <ipt/ipt.h>
#include <ipt/sharedmem.h>

#include "VehPoseSource.h"

#include <VehPoseDest/VehPoseStructs.h>

class ShmemVehPoseSource : public VehPoseSource {
public:
  ShmemVehPoseSource();
  virtual ~ShmemVehPoseSource();

  virtual bool getPose(utils::Time time, VehPose& pose);

  virtual bool getCurPose(utils::Time& time,
                          VehPose& pose, bool blocking = false);

  bool init(utils::ConfigFile& params, utils::SymbolTable* globals);

  static void interpolate(const VehPoseShmemStruct& prev_pose, 
                          const VehPoseShmemStruct& next_pose, double t,
                          VehPose& veh_pose);


private:
  static void* thread_entry(void*);
  void collector_thread();
  void error(VehPose&); 
  static void set_pose(const VehPoseShmemStruct& input, VehPose& output);

private:
  IPCommunicator* _com;  // the IPT communicator
  IPSharedMemory* _shm;  // the pose source shared memory region

  bool _collector_running;  // true if the collector thread is running
  pthread_t _collector_t;  // the collector thread ID
  pthread_mutex_t _collector_mutex;  // mutex which guards ring buffer
  pthread_cond_t _collector_cond;  // conditional which signals new pose

  int _history_length;  // the size of the pose ring buffer
  int _num_poses;      // number of poses in the ring buffer
  int _cur_pose_index;  // most recent pose index in the ring buffer
  VehPoseShmemStruct* _poses;  // the ring buffer

  int _last_secs, _last_usecs;  // time stamp of the last pose received
                                // by getCurPose
  float _max_extrapolation;  // how far can we extrapolate pose information?
};

ShmemVehPoseSource::ShmemVehPoseSource()
{
  _shm = NULL;
  _collector_running = false;
  _poses = NULL;
  _last_secs = _last_usecs = -1;
}

ShmemVehPoseSource::~ShmemVehPoseSource()
{
  // close shared memory, if necessary
  if (_shm)
    _com->CloseSharedMemory(_shm);

  // shutdown collector thread and cleanup, if necessary
  if (_collector_running) {
    _collector_running = false;
    pthread_cancel(_collector_t);
    pthread_mutex_destroy(&_collector_mutex);
    pthread_cond_destroy(&_collector_cond);
  }

  delete [] _poses;
}

void ShmemVehPoseSource::interpolate(const VehPoseShmemStruct& prev_pose, 
                                     const VehPoseShmemStruct& cur_pose,
                                     double t, VehPose& veh_pose)
{
  VehPose prev_real_pose, cur_real_pose;
  set_pose(prev_pose, prev_real_pose);
  set_pose(cur_pose, cur_real_pose);
  VehPoseSource::interpolate(prev_real_pose, cur_real_pose, t, veh_pose);
}

bool ShmemVehPoseSource::init(utils::ConfigFile& params,
                              utils::SymbolTable* globals)
{
  // IPCommunicator::Communicator creates an IPT communicator only if
  // one has not been created and cached in globals already.
  // If it does create one, it caches it in globals.
  _com =
    IPCommunicator::Communicator(globals, 
                                 params.getString("ipt_spec",
                                                   "unix:port=0;"));
  if (!_com)
    return false;

  // create the shared memory region
  // for convenience, the user can just specify name
  const char* mem_name = params.getString("name", VEH_POSE_SHMEM_NAME);
  // optionally, machine (empty means on local host
  const char* machine = params.getString("machine");
  // and the port that the memory manager is running on
  int port = params.getInt("port", 1389);
  char buffer[200];
  // to create a default memory specification for managed shared memory
  if (!*machine) {
    sprintf(buffer, "managed: name=%s;", mem_name);
  } else {
    sprintf(buffer, "managed: name='%s@%s|%d';", mem_name, machine, port);
  }
  // of course, this can be overridden through explicit specification of "mem"
  const char* mem_spec = params.getString("mem", buffer);
  _shm =
    _com->OpenSharedMemory(mem_spec, VEH_POSE_SHMEM_FMT,
                          sizeof(VehPoseShmemStruct));
  // if we can't open shared memory
  if (!_shm) {
    // print an error and return bad
    fprintf(stderr,
            "ShmemVehPoseSource::init: Problem opening shared memory %s\n",
            mem_spec);
    return false;
  }

  // now create and set up the ring buffer
  _history_length = params.getInt("history_length",  100);
  _num_poses = 0;
  _cur_pose_index = -1;
  _poses = new VehPoseShmemStruct[_history_length];

  _max_extrapolation = params.getFloat("max_extrapolation", 0.2);

  // and finally, kick off the collector thread
  pthread_mutex_init(&_collector_mutex, NULL);
  pthread_cond_init(&_collector_cond, NULL);
  _collector_running = true;
  pthread_create(&_collector_t, NULL, thread_entry, this);

  return true;
}

// entry point for the collector thread
void* ShmemVehPoseSource::thread_entry(void* data)
{
  ((ShmemVehPoseSource*) data)->collector_thread();
  return NULL;
}

// the collector thread
void ShmemVehPoseSource::collector_thread()
{
  VehPoseShmemStruct incoming;
  while (_collector_running) {
    // wait for new data in the shared memory
    if (!_shm->Wait()) {
      utils::Time::sleep(0.02);  // make sure we don't busy wait
      continue;
    }

    // unmarshal it and stick pose in incoming
    _shm->FormattedData((void*) &incoming);
    // the time tag of the pose can get set to 0 on startup and shutdown
    // of the module producing the poses.  Just skip these.
    if (!incoming.secs && !incoming.usecs)
      continue;

    // Put the new pose in the ring buffer
    pthread_mutex_lock(&_collector_mutex);
    if (_num_poses != _history_length)
      _num_poses++;
    _cur_pose_index = (_cur_pose_index + 1) % _history_length;
    _poses[_cur_pose_index] = incoming;
    pthread_mutex_unlock(&_collector_mutex);

    // signal we have new data for anyone blocking in getCurPose
    pthread_cond_signal(&_collector_cond);
  }
}

// convenience method for invalidating sensor pose and releasing the
// collector mutex
void ShmemVehPoseSource::error(VehPose& veh_pose)
{
  veh_pose.pos = utils::Vec3d();
  veh_pose.ori = utils::Rotation();
  pthread_mutex_unlock(&_collector_mutex);
}

// convenience method for setting a sensor pose output from the data in the
// shared memory region input
void ShmemVehPoseSource::set_pose(const VehPoseShmemStruct& input,
                                  VehPose& output)
{
  output.pos = utils::Vec3d(input.data.x, input.data.y, input.data.z);
  output.ori = utils::Rotation(input.data.ori[0], input.data.ori[1], 
                               input.data.ori[2], input.data.ori[3]);
}

// Lookup the sensor pose at time now and put it in sensor pose, if possible
bool ShmemVehPoseSource::getPose(utils::Time now, VehPose& veh_pose)
{
  // lock the ring buffer
  pthread_mutex_lock(&_collector_mutex);

  // Get the latest sensor pose
  VehPoseShmemStruct* cur_pose = &_poses[_cur_pose_index];
  utils::Time t(cur_pose->secs, cur_pose->usecs);

  // if the requested time is after the latest sensor pose
  if (now > t) {
    // figure out if we can extrapolate
    double elapsed = (now - t).getValue();
    if (elapsed > _max_extrapolation) {
      fprintf(stderr, "ShmemVehPoseSource::getPose: "
              "Pose lookup time too far in the future, delta = %f\n",
              (now - t).getValue());
      error(veh_pose);
      return false;
    }
    // default all fields to cur_pose value.
    set_pose(*cur_pose, veh_pose);

    // get previous pose
    VehPoseShmemStruct* prev_pose;
    utils::Time pt;
    int prev_index = _cur_pose_index;
    while (1) {
      prev_index--;
      if (prev_index < 0 && _num_poses < _history_length) {
        fprintf(stderr, "ShmemVehPoseSource::getPose: "
                "Cannot extrapolate yet (%5.2f)\n",
                (now - t).getValue());
        error(veh_pose);
        return false;
      }
      prev_pose = &_poses[prev_index];
      pt.setValue(prev_pose->secs, prev_pose->usecs); 
      if (pt > t) {
        fprintf(stderr, "ShmemVehPoseSource::getPose: "
                "Cannot extrapolate with current history (%5.2f)\n",
                (now - t).getValue());
        error(veh_pose);
        return false;
      }
      if (pt != t)
        break;
    }
    
    // extrapolate pose (with t > 1)
    interpolate(*prev_pose, *cur_pose, 1 + double(now-t)/(t-pt), veh_pose);

    // unlock the ring buffer and return
    pthread_mutex_unlock(&_collector_mutex);
    return true;
  }

  // go through the ring buffer looking for the elements bracketing
  // the requested time
  int cur_index = _cur_pose_index;
  VehPoseShmemStruct* prev_pose = cur_pose;
  for (int i=0;i<_num_poses-1;i++) {
    cur_index--;
    if (cur_index < 0)
      cur_index = _history_length-1;
    prev_pose = &_poses[cur_index];
    t.setValue(prev_pose->secs, prev_pose->usecs);
    if (now > t) {
      // we have found the bracketing elements

      utils::Time cur(cur_pose->secs, cur_pose->usecs);
      double dist = (now-t).getValue()/(cur-t).getValue();

      // interpolate appropriately
      interpolate(*prev_pose, *cur_pose, dist, veh_pose);

      // unlock the ring buffer and return
      pthread_mutex_unlock(&_collector_mutex);
      return true;
    }
    cur_pose = prev_pose;
  }

  // requrest time is too far in the past
  fprintf(stderr, "ShmemVehPoseSource::getPose: "
          "State lookup time too old, delta = %f\n",
          (now - TimeSource::now()).getValue());
  error(veh_pose);

  return false;
}

// get the latest sensor pose, blocking if necessary
bool ShmemVehPoseSource::getCurPose(utils::Time& time, VehPose& veh_pose,
                                    bool blocking)
{
  // lock the ring buffer
  pthread_mutex_lock(&_collector_mutex);

  if (blocking) {
    // if we are not at the first one and do not have new data
    if (_cur_pose_index < 0 ||
        (_poses[_cur_pose_index].secs == _last_secs &&
         _poses[_cur_pose_index].usecs == _last_usecs)) {
      // wait for new data
      if (pthread_cond_wait(&_collector_cond, &_collector_mutex)) {
        pthread_mutex_unlock(&_collector_mutex);
        perror("ShmemVehPoseSource::getCurPose: waiting for condition");
        error(veh_pose);
        return false;
      }
    }
  } 

  // get the latest sensor pose
  VehPoseShmemStruct& result = _poses[_cur_pose_index];
  set_pose(result, veh_pose);
  time.setValue(result.secs, result.usecs);

  // mark if this is new data or not
  bool res = !(_last_secs == result.secs && _last_usecs == result.usecs);
  _last_secs = result.secs;
  _last_usecs = result.usecs;

  // unlock the ring buffer
  pthread_mutex_unlock(&_collector_mutex);

  return res;
}

UTILS_INTF_CREATOR(VehPoseSource, shmem, gen, params, globals)
{
  UTILS_INTF_REPORT(VehPoseSource, shmem);
  ShmemVehPoseSource* player = new ShmemVehPoseSource();
  if (!player->init(*params, globals)) {
    delete player;
    return NULL;
  }
  return player;
}

---