42 #include <visp/vpConfig.h>
43 #include <visp/vpMath.h>
44 #include <visp/vpRobotException.h>
45 #include <std_srvs/Empty.h>
97 throw (vpRobotException(vpRobotException::notInitialized,
98 "ROS robot pioneer is not initialized") );
103 throw(vpRobotException(vpRobotException::dimensionError,
"Velocity vector is not a 2 dimension vector"));
106 vpColVector vel_max(2);
108 vpColVector vel_robot(6);
110 std::cout <<
"vel recu: " << vel << std::endl;
111 if (frame == vpRobot::REFERENCE_FRAME)
113 vel_max[0] = getMaxTranslationVelocity();
114 vel_max[1] = getMaxRotationVelocity();
116 vel_sat = vpRobot::saturateVelocities(vel, vel_max,
true);
117 vel_robot[0] = vel_sat[0];
122 vel_robot[5] = vel_sat[1];
123 std::cout <<
"vel envoye: " << vel_robot << std::endl;
128 throw vpRobotException (vpRobotException::wrongStateError,
129 "Cannot send the robot velocity in the specified control frame");
135 ros::ServiceClient client =
n->serviceClient<std_srvs::Empty>(
"/RosAria/enable_motors");
137 if (!client.call(srv))
139 ROS_ERROR(
"Unable to enable motors");
145 ros::ServiceClient client =
n->serviceClient<std_srvs::Empty>(
"/RosAria/disable_motors");
147 if (!client.call(srv))
149 ROS_ERROR(
"Unable to disable motors");
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void init()
basic initialization
void init()
basic initialization
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
~vpROSRobotPioneer()
destructor