vpROSRobotPioneer.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * $Id: vpROSRobotPioneer.cpp 4574 2014-01-09 08:48:51Z fpasteau $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for Pioneer robots based on ROS middleware.
36  *
37  * Authors:
38  * Francois Pasteau
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpConfig.h>
43 #include <visp/vpMath.h>
44 #include <visp/vpRobotException.h>
45 #include <std_srvs/Empty.h>
47 
48 
53 {
54 
55 }
56 
59 {
60  stopMotion();
61  disableMotors();
62 }
63 
66 {
68  enableMotors();
69 }
70 
71 void vpROSRobotPioneer::init(int argc, char **argv)
72 {
73  vpROSRobot::init(argc, argv);
74  enableMotors();
75 }
76 
94 void vpROSRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
95 {
96  if (! isInitialized) {
97  throw (vpRobotException(vpRobotException::notInitialized,
98  "ROS robot pioneer is not initialized") );
99  }
100 
101  if (vel.size() != 2)
102  {
103  throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
104  }
105 
106  vpColVector vel_max(2);
107  vpColVector vel_sat;
108  vpColVector vel_robot(6);
109 
110  std::cout << "vel recu: " << vel << std::endl;
111  if (frame == vpRobot::REFERENCE_FRAME)
112  {
113  vel_max[0] = getMaxTranslationVelocity();
114  vel_max[1] = getMaxRotationVelocity();
115 
116  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
117  vel_robot[0] = vel_sat[0];
118  vel_robot[1] = 0;
119  vel_robot[2] = 0;
120  vel_robot[3] = 0;
121  vel_robot[4] = 0;
122  vel_robot[5] = vel_sat[1];
123  std::cout << "vel envoye: " << vel_robot << std::endl;
124  vpROSRobot::setVelocity(frame, vel_robot);
125  }
126  else
127  {
128  throw vpRobotException (vpRobotException::wrongStateError,
129  "Cannot send the robot velocity in the specified control frame");
130  }
131 }
132 
134 {
135  ros::ServiceClient client = n->serviceClient<std_srvs::Empty>("/RosAria/enable_motors");
136  std_srvs::Empty srv;
137  if (!client.call(srv))
138  {
139  ROS_ERROR("Unable to enable motors");
140  }
141 }
142 
144 {
145  ros::ServiceClient client = n->serviceClient<std_srvs::Empty>("/RosAria/disable_motors");
146  std_srvs::Empty srv;
147  if (!client.call(srv))
148  {
149  ROS_ERROR("Unable to disable motors");
150  }
151 }
152 
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
void init()
basic initialization
ros::NodeHandle * n
Definition: vpROSRobot.h:65
bool call(MReq &req, MRes &res)
void stopMotion()
Definition: vpROSRobot.cpp:265
bool isInitialized
Definition: vpROSRobot.h:70
void init()
basic initialization
Definition: vpROSRobot.cpp:112
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpROSRobot.cpp:144
#define ROS_ERROR(...)
~vpROSRobotPioneer()
destructor


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais
autogenerated on Fri Jul 3 2020 03:41:43