39 #include <visp3/core/vpConfig.h> 41 #include <visp3/robot/vpRobotPioneer.h> 42 #include <visp3/robot/vpRobotBiclops.h> 43 #include <visp3/core/vpCameraParameters.h> 44 #include <visp3/gui/vpDisplayGDI.h> 45 #include <visp3/gui/vpDisplayX.h> 46 #include <visp3/blob/vpDot2.h> 47 #include <visp3/visual_features/vpFeatureBuilder.h> 48 #include <visp3/visual_features/vpFeatureSegment.h> 49 #include <visp3/core/vpHomogeneousMatrix.h> 50 #include <visp3/core/vpImage.h> 51 #include <visp3/sensor/vp1394TwoGrabber.h> 52 #include <visp3/sensor/vp1394CMUGrabber.h> 53 #include <visp3/sensor/vpV4l2Grabber.h> 54 #include <visp3/robot/vpPioneerPan.h> 55 #include <visp3/gui/vpPlot.h> 56 #include <visp3/vs/vpServo.h> 57 #include <visp3/core/vpVelocityTwistMatrix.h> 59 #define USE_REAL_ROBOT 61 #undef VISP_HAVE_V4L2 // To use a firewire camera 80 #if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS) 81 int main(
int argc,
char **argv)
83 #if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) 84 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) 90 double coef = 1.2/13.0;
93 bool normalized =
true;
102 #ifdef USE_REAL_ROBOT 126 ArArgumentParser parser(&argc, argv);
127 parser.loadDefaultArguments();
131 ArRobotConnector robotConnector(&parser, &pioneer);
132 if(!robotConnector.connectRobot())
134 ArLog::log(ArLog::Terse,
"Could not connect to the pioneer robot.");
135 if(parser.checkHelpAndWarnUnparsed())
141 if (!Aria::parseArgs())
156 std::cout <<
"Pioneer robot connected" << std::endl;
165 #if defined(VISP_HAVE_V4L2) 174 #elif defined(VISP_HAVE_DC1394) 181 #elif defined(VISP_HAVE_CMU1394) 195 #if defined(VISP_HAVE_X11) 197 #elif defined(VISP_HAVE_GDI) 205 for (
int i=0; i <2; i++)
224 std::cout <<
"cVe: \n" << cVe << std::endl;
233 std::cout <<
"eJe: \n" << eJe << std::endl;
241 for (
int i=0; i <2; i++)
250 for (
int i=0; i<2; i++)
253 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
256 Z[i] = coef * surface[i] ;
262 s_segment.setZ1(Z[0]);
263 s_segment.setZ2(Z[1]);
266 s_segment.setZ1( P[0].get_Z() );
267 s_segment.setZ2( P[1].get_Z() );
276 vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
279 graph.initGraph(0,3);
280 graph.initGraph(1,3);
281 graph.setTitle(0,
"Velocities");
282 graph.setTitle(1,
"Error s-s*");
283 graph.setLegend(0, 0,
"vx");
284 graph.setLegend(0, 1,
"wz");
285 graph.setLegend(0, 2,
"w_pan");
286 graph.setLegend(1, 0,
"xm/l");
287 graph.setLegend(1, 1,
"1/l");
288 graph.setLegend(1, 2,
"alpha");
292 unsigned int iter = 0;
297 #ifdef USE_REAL_ROBOT 309 for (
int i=0; i<2; i++)
313 for (
int i=0; i<2; i++)
316 for (
int i=0; i<2; i++)
319 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
322 Z[i] = coef * surface[i] ;
329 s_segment.setZ1(Z[0]);
330 s_segment.setZ2(Z[1]);
354 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
357 graph.plot(0, iter, v);
358 graph.plot(1, iter, task.
getError());
361 #ifdef USE_REAL_ROBOT 370 std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s " 371 <<
vpMath::deg(v_pioneer[1]) <<
" deg/s" << std::endl;
372 std::cout <<
"Send velocity to the biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
394 #ifdef USE_REAL_ROBOT 395 std::cout <<
"Ending robot thread..." << std::endl;
396 pioneer.stopRunning();
399 pioneer.waitForRunExit();
407 std::cout <<
"Catch an exception: " << e << std::endl;
416 std::cout <<
"ViSP is not able to control the Pioneer robot" << std::endl;
Implementation of a matrix and operations on matrices.
static unsigned int selectAlpha()
void setVideoMode(unsigned long format, unsigned long mode)
void open(vpImage< unsigned char > &I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void open(vpImage< unsigned char > &I)
void useSonar(bool usage)
Display for windows using GDI (available on any windows 32 platform).
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
void set_eJe(const vpMatrix &eJe_)
vpVelocityTwistMatrix get_cVe() const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Define the X11 console to display images.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
void setDevice(const std::string &devname)
Initialize the position controller.
error that can be emited by ViSP classes.
Interface for Pioneer mobile robots based on Aria 3rd party library.
Generic functions for Pioneer mobile robots equiped with a pan head.
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
static void flush(const vpImage< unsigned char > &I)
Class that defines what is a point.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void setGrayLevelPrecision(const double &grayLevelPrecision)
Initialize the velocity controller.
vpColVector getError() const
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setFramerate(unsigned long fps)
vpColVector computeControlLaw()
static unsigned int selectL()
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
void acquire(vpImage< unsigned char > &I)
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void setComputeMoments(const bool activate)
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
Interface for the biclops, pan, tilt head control.
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Class for the Video4Linux2 video device.
static double deg(double rad)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Class for firewire ieee1394 video devices using libdc1394-2.x api.
virtual bool getClick(bool blocking=true)=0
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
void setServo(const vpServoType &servo_type)
void set_eJe(double q_pan)
static unsigned int selectXc()
unsigned int getTaskRank() const
void setGraphics(const bool activate)
void setFramerate(vpV4l2FramerateType framerate)