ati_force_sensor_driver.cpp

Go to the documentation of this file.

#include "ati_force_sensor_driver.h"
#include "ati_force_sensor_exceptions.h"

AtiForceSensorDriver::AtiForceSensorDriver(void) :
  filter_cutt_off_freq_(73)
{
  //setDriverId(driver string id);
  sensor_ = boost::shared_ptr<CAti_Force_Sensor>(new CAti_Force_Sensor("ati_sensor","192.168.101.23"));
//  sensor_ = new CAti_Force_Sensor("ati_sensor","192.168.101.23");
}

bool AtiForceSensorDriver::openDriver(void)
{
  //setDriverId(driver string id);
    sensor_->cancel_offsets();
    sensor_->get_system_info(&sys_info_);
    sensor_->get_calibration_info(&cal_info_);
    sensor_->set_filter_cutt_off_freq(filter_cutt_off_freq_);
  return true;
}

bool AtiForceSensorDriver::closeDriver(void)
{
  return true;
}

bool AtiForceSensorDriver::startDriver(void)
{

  sensor_->start();
  return true;
}

bool AtiForceSensorDriver::stopDriver(void)
{
  if (this->getState() != AtiForceSensorDriver::CLOSED)  
    sensor_->stop();
  return true;
}

void AtiForceSensorDriver::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();
  
  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case AtiForceSensorDriver::CLOSED:
      break;

    case AtiForceSensorDriver::OPENED:
      break;

    case AtiForceSensorDriver::RUNNING:
      try
      {
        sensor_->stop();
        sensor_->set_filter_cutt_off_freq(new_cfg.filter_cutt_off_freq);
        sensor_->start();
        filter_cutt_off_freq_ = new_cfg.filter_cutt_off_freq;
      }
      catch (CException &e)
      {
        ROS_ERROR("ERROR: %s",e.what().c_str());
        new_cfg.filter_cutt_off_freq = filter_cutt_off_freq_;

      }
      break;
  }

  // save the current configuration
  this->config_=new_cfg;

  this->unlock();
}

std::string AtiForceSensorDriver::getNewSampleEventId(void) const
{
  return sensor_->get_new_sample_event_id();
}

geometry_msgs::WrenchStamped AtiForceSensorDriver::getData(void)
{
//  if (this->getState() != AtiForceSensorDriver::CLOSED) 
//    return;

  std::vector<double> force,torque;
  geometry_msgs::WrenchStamped dataMsg;

  sensor_->get_sample(force,torque);
    
  //dataMsg.header.seq=seq_counter++;
  dataMsg.header.stamp    = ros::Time::now();
  dataMsg.header.frame_id = config_.frame_id;
  dataMsg.wrench.force.x = force[0];
  dataMsg.wrench.force.y = force[1];
  dataMsg.wrench.force.z = force[2];
  dataMsg.wrench.torque.x = torque[0];
  dataMsg.wrench.torque.y = torque[1];
  dataMsg.wrench.torque.z = torque[2];

  return dataMsg;
}

AtiForceSensorDriver::~AtiForceSensorDriver(void)
{
}