vpROSRobotPioneer.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: vpROSRobotPioneer.cpp 4574 2014-01-09 08:48:51Z fpasteau $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
00007  *
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  *
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  *
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  *
00034  * Description:
00035  * Interface for Pioneer robots based on ROS middleware.
00036  *
00037  * Authors:
00038  * Francois Pasteau
00039  *
00040  *****************************************************************************/
00041 
00042 #include <visp/vpConfig.h>
00043 #include <visp/vpMath.h>
00044 #include <visp/vpRobotException.h>
00045 #include <std_srvs/Empty.h>
00046 #include <visp_ros/vpROSRobotPioneer.h>
00047 
00048 
00052 vpROSRobotPioneer::vpROSRobotPioneer() : vpPioneer()
00053 {
00054 
00055 }
00056 
00058 vpROSRobotPioneer::~vpROSRobotPioneer()
00059 {
00060   stopMotion();
00061   disableMotors();
00062 }
00063 
00065 void vpROSRobotPioneer::init()
00066 {
00067   vpROSRobot::init();
00068   enableMotors();
00069 }
00070 
00071 void vpROSRobotPioneer::init(int argc, char **argv)
00072 {
00073   vpROSRobot::init(argc, argv);
00074   enableMotors();
00075 }
00076 
00094 void vpROSRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
00095 {
00096   if (! isInitialized) {
00097     throw (vpRobotException(vpRobotException::notInitialized,
00098                             "ROS robot pioneer is not initialized") );
00099   }
00100 
00101   if (vel.size() != 2)
00102   {
00103     throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
00104   }
00105 
00106   vpColVector vel_max(2);
00107   vpColVector vel_sat;
00108   vpColVector vel_robot(6);
00109 
00110   std::cout << "vel recu: " << vel << std::endl;
00111   if (frame == vpRobot::REFERENCE_FRAME)
00112   {
00113     vel_max[0] = getMaxTranslationVelocity();
00114     vel_max[1] = getMaxRotationVelocity();
00115 
00116     vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
00117     vel_robot[0] = vel_sat[0];
00118     vel_robot[1] = 0;
00119     vel_robot[2] = 0;
00120     vel_robot[3] = 0;
00121     vel_robot[4] = 0;
00122     vel_robot[5] = vel_sat[1];
00123     std::cout << "vel envoye: " << vel_robot << std::endl;
00124     vpROSRobot::setVelocity(frame, vel_robot);
00125   }
00126   else
00127   {
00128     throw vpRobotException (vpRobotException::wrongStateError,
00129                             "Cannot send the robot velocity in the specified control frame");
00130   }
00131 }
00132 
00133 void vpROSRobotPioneer::enableMotors()
00134 {
00135   ros::ServiceClient client = n->serviceClient<std_srvs::Empty>("/RosAria/enable_motors");
00136   std_srvs::Empty srv;
00137   if (!client.call(srv))
00138   {
00139     ROS_ERROR("Unable to enable motors");
00140   }
00141 }
00142 
00143 void vpROSRobotPioneer::disableMotors()
00144 {
00145   ros::ServiceClient client = n->serviceClient<std_srvs::Empty>("/RosAria/disable_motors");
00146   std_srvs::Empty srv;
00147   if (!client.call(srv))
00148   {
00149     ROS_ERROR("Unable to disable motors");
00150   }
00151 }
00152 


visp_ros
Author(s): Francois Pasteau, Fabien Spindler
autogenerated on Thu Mar 24 2016 03:32:49