vpRobotPioneer.cpp

/****************************************************************************
 *
 * $Id: vpRobotPioneer.cpp 4574 2014-01-09 08:48:51Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Interface for Pioneer robots based on Aria 3rd party library.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp/vpConfig.h>
#include <visp/vpMath.h>
#include <visp/vpRobotException.h>
#include <visp/vpRobotPioneer.h>

#ifdef VISP_HAVE_PIONEER

vpRobotPioneer::vpRobotPioneer() : vpPioneer(), ArRobot()
{
  isInitialized = false;

  Aria::init();
}

vpRobotPioneer::~vpRobotPioneer()
{
#if 0
  std::cout << "Ending robot thread..." << std::endl;
  stopRunning();

  // wait for the thread to stop
  waitForRunExit();
#endif
}

void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
  init();

  /*
  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
    vpERROR_TRACE ("Cannot send a velocity to the robot "
       "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
    throw vpRobotException (vpRobotException::wrongStateError,
          "Cannot send a velocity to the robot "
          "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
  } */

  if (vel.size() != 2)
  {
    throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
  }

  vpColVector vel_max(2);
  vpColVector vel_sat;

  if (frame == vpRobot::REFERENCE_FRAME)
  {
    vel_max[0] = getMaxTranslationVelocity();
    vel_max[1] = getMaxRotationVelocity();

    vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
    this->lock();
    this->setVel(vel_sat[0]*1000.); // convert velocity in mm/s
    this->setRotVel( vpMath::deg(vel_sat[1]) ); // convert velocity in deg/s
    this->unlock();
  }
  else if (frame == vpRobot::ARTICULAR_FRAME)
  {
    vel_max[0] = getMaxTranslationVelocity();
    vel_max[1] = getMaxTranslationVelocity();

    vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
    this->lock();
    //std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
    this->setVel2(vel_sat[0]*1000., vel_sat[1]*1000.); // convert velocity in mm/s
    this->unlock();
  }
  else
  {
    throw vpRobotException (vpRobotException::wrongStateError,
                            "Cannot send the robot velocity in the specified control frame");
  }
}

void vpRobotPioneer::init()
{
  if ( ! isInitialized )
  {
    // Start the robot processing cycle running in the background.
    // True parameter means that if the connection is lost, then the
    // run loop ends.
    this->runAsync(true);
    this->lock();
    this->enableMotors();
    this->unlock();

    isInitialized = true;
  }
}

void vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & velocity)
{
  init();
  velocity.resize(2);

  if (frame == vpRobot::ARTICULAR_FRAME)
  {
    this->lock();
    velocity[0] =   this->getLeftVel() / 1000.;
    velocity[1] =   this->getRightVel() / 1000;
    this->unlock();
  }
  else if (frame == vpRobot::REFERENCE_FRAME)
  {
    this->lock();
    velocity[0] = this->getVel() / 1000.;
    velocity[1] = vpMath::rad( this->getRotVel() );
    this->unlock();
  }
  else {
    throw vpRobotException (vpRobotException::wrongStateError,
                            "Cannot get the robot volocity in the specified control frame");
  }
}

vpColVector vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame)
{
  vpColVector velocity;
  getVelocity(frame, velocity);
  return velocity;
}

#endif