darwin_robot_driver.cpp

Go to the documentation of this file.

#include "darwin_robot_driver.h"
#include "darwin_exceptions.h"

DarwinRobotDriver::DarwinRobotDriver(void)
{
  //setDriverId(driver string id);
  this->robot=NULL;

  this->CM730_device="/dev/ttyUSB0";
  this->action_file="/home/darwin/darwinop/Data/motion_4096.bin";
  this->config_file="/home/darwin/iridrivers/darwin_robot/trunk/config.ini";
}

bool DarwinRobotDriver::openDriver(void)
{
  //setDriverId(driver string id);
  try{
    this->robot=new CDarwin(this->CM730_device,this->action_file);
    this->robot->load_config(this->config_file);
    return true;
  }catch(CException &e){
    ROS_INFO("%s",e.what().c_str());
    if(this->robot!=NULL)
    {
      delete this->robot;
      this->robot=NULL;
    }
    return false;
  }
}

bool DarwinRobotDriver::closeDriver(void)
{
  if(this->robot!=NULL)
  {
    this->robot->stop_walking();
    this->robot->stop_action();
    this->robot->stop_all_joints();
    delete this->robot;
    this->robot=NULL;
  }

  return true;
}

bool DarwinRobotDriver::startDriver(void)
{
  return true;
}

bool DarwinRobotDriver::stopDriver(void)
{
  return true;
}

void DarwinRobotDriver::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();
  
  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case DarwinRobotDriver::CLOSED:
      if(this->robot!=NULL)
      {
        // change the motion and configuration files
        try{
          this->robot->load_config(new_cfg.config_file);
          this->config_file=new_cfg.config_file;
        }catch(CDarwinException &e){
          ROS_INFO("%s",e.what().c_str());
          new_cfg.config_file=this->config_file;
        }
        try{
          this->robot->load_actions(new_cfg.motion_file);
          this->action_file=new_cfg.motion_file;
        }catch(CDarwinException &e){
          ROS_INFO("%s",e.what().c_str());
          new_cfg.motion_file=this->action_file;
        }
      }
      break;

    case DarwinRobotDriver::OPENED:
      // change the USB device port
      this->CM730_device=new_cfg.device_id;
      break;

    case DarwinRobotDriver::RUNNING:
      break;
  }

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

  this->unlock();
}

std::vector<double> DarwinRobotDriver::get_current_angles(void)
{
  return this->robot->get_current_angles();
}

// Walking interface
void DarwinRobotDriver::start_walking(double x_dist, double y_dist, double turn_angle)
{
  this->robot->start_walking(x_dist,y_dist,turn_angle);
}

void DarwinRobotDriver::tune_walking(double x_dist,double y_dist, double turn_angle)
{
  this->robot->tune_walking(x_dist,y_dist,turn_angle);
}

void DarwinRobotDriver::set_walking_param(motion_params param, double value)
{
  this->robot->set_walking_param(param,value);
}

double DarwinRobotDriver::get_walking_param(motion_params param)
{
  return this->robot->get_walking_param(param);
}

void DarwinRobotDriver::stop_walking(void)
{
  this->robot->stop_walking();
}

bool DarwinRobotDriver::is_walking(void)
{
  return this->robot->is_walking();
}

// action interface
void DarwinRobotDriver::start_action(unsigned int action_index)
{
  this->robot->start_action(action_index);
}

void DarwinRobotDriver::start_action(std::string &action_name)
{
  this->robot->start_action(action_name);
}

void DarwinRobotDriver::stop_action(void)
{
  this->robot->stop_action();
}

void DarwinRobotDriver::break_action(void)
{
  this->robot->break_action();
}

bool DarwinRobotDriver::action_is_running(void)
{
  return this->robot->action_is_running();
}

// Head public interface
void DarwinRobotDriver::get_head_position(double *pan, double *tilt)
{
  this->robot->get_head_position(pan,tilt);
}

void DarwinRobotDriver::set_head_position(double pan, double tilt)
{
  this->robot->set_head_position(pan,tilt);
}

// sensor interface
void DarwinRobotDriver::get_gyroscope(double *x,double *y, double *z)
{
  this->robot->get_gyroscope(x,y,z);
}

void DarwinRobotDriver::get_accelerometer(double *x, double *y, double *z)
{
  this->robot->get_accelerometer(x,y,z);
}

void DarwinRobotDriver::get_analog_channel(analogs id,double *value)
{
  this->robot->get_analog_channel(id,value);
}

// joint motion public interface
std::string DarwinRobotDriver::move_joints(std::vector<int> &servos,std::vector<double> &angles,std::vector<double> &speeds, std::vector<double> &accels,const std::string &target_id)
{
  return this->robot->move_joints(servos,angles,speeds,accels,target_id);
}

bool DarwinRobotDriver::joints_are_moving(std::string &target_id)
{
  return this->robot->joints_are_moving(target_id);
}

void DarwinRobotDriver::stop_joints(std::string &target_id)
{
  this->robot->stop_joints(target_id);
}

// tracking public interface
void DarwinRobotDriver::start_head_tracking(void)
{
  this->robot->start_head_tracking();
}

void DarwinRobotDriver::stop_head_tracking(void)
{
  this->robot->stop_head_tracking();
}

void DarwinRobotDriver::set_tracking_pid(int t, TPID *pan_pid, TPID *tilt_pid)
{
  this->robot->set_tracking_pid(t,pan_pid,tilt_pid);
}

void DarwinRobotDriver::get_tracking_pid(int *t, TPID *pan_pid, TPID *tilt_pid)
{
  this->robot->get_tracking_pid(t,pan_pid,tilt_pid);
}

void DarwinRobotDriver::set_tracking_target(double pan, double tilt)
{
  this->robot->set_tracking_target(pan,tilt);
}

void DarwinRobotDriver::get_tracking_target(double *pan, double *tilt)
{
  this->robot->get_tracking_target(pan,tilt);
}

DarwinRobotDriver::~DarwinRobotDriver(void)
{
  this->lock();
  if(this->robot!=NULL)
  {
    this->robot->stop_walking();
    this->robot->stop_action();
    this->robot->stop_all_joints();
    delete this->robot;
    this->robot=NULL;
  }
  this->unlock();
}