43 #include <visp/vpConfig.h>
45 #include <visp/vpRobotPioneer.h>
46 #include <visp/vpRobotBiclops.h>
47 #include <visp/vpCameraParameters.h>
48 #include <visp/vpDisplayGDI.h>
49 #include <visp/vpDisplayX.h>
50 #include <visp/vpDot2.h>
51 #include <visp/vpFeatureBuilder.h>
52 #include <visp/vpFeatureSegment.h>
53 #include <visp/vpHomogeneousMatrix.h>
54 #include <visp/vpImage.h>
55 #include <visp/vp1394TwoGrabber.h>
56 #include <visp/vp1394CMUGrabber.h>
57 #include <visp/vpV4l2Grabber.h>
58 #include <visp/vpPioneerPan.h>
59 #include <visp/vpPlot.h>
60 #include <visp/vpServo.h>
61 #include <visp/vpVelocityTwistMatrix.h>
63 #define USE_REAL_ROBOT
65 #undef VISP_HAVE_V4L2 // To use a firewire camera
84 #if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
85 int main(
int argc,
char **argv)
87 #if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
88 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
93 double coef = 1.2/13.0;
97 bool normalized =
true;
106 #ifdef USE_REAL_ROBOT
130 ArArgumentParser parser(&argc, argv);
131 parser.loadDefaultArguments();
135 ArRobotConnector robotConnector(&parser, &pioneer);
136 if(!robotConnector.connectRobot())
138 ArLog::log(ArLog::Terse,
"Could not connect to the pioneer robot.");
139 if(parser.checkHelpAndWarnUnparsed())
145 if (!Aria::parseArgs())
160 std::cout <<
"Pioneer robot connected" << std::endl;
169 #if defined(VISP_HAVE_V4L2)
178 #elif defined(VISP_HAVE_DC1394_2)
185 #elif defined(VISP_HAVE_CMU1394)
199 #if defined(VISP_HAVE_X11)
201 #elif defined(VISP_HAVE_GDI)
209 for (
int i=0; i <2; i++)
228 std::cout <<
"cVe: \n" << cVe << std::endl;
237 std::cout <<
"eJe: \n" << eJe << std::endl;
245 for (
int i=0; i <2; i++)
254 for (
int i=0; i<2; i++)
257 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
260 Z[i] = coef * surface[i] ;
266 s_segment.setZ1(Z[0]);
267 s_segment.setZ2(Z[1]);
270 s_segment.setZ1( P[0].get_Z() );
271 s_segment.setZ2( P[1].get_Z() );
280 vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
283 graph.initGraph(0,3);
284 graph.initGraph(1,3);
285 graph.setTitle(0,
"Velocities");
286 graph.setTitle(1,
"Error s-s*");
287 graph.setLegend(0, 0,
"vx");
288 graph.setLegend(0, 1,
"wz");
289 graph.setLegend(0, 2,
"w_pan");
290 graph.setLegend(1, 0,
"xm/l");
291 graph.setLegend(1, 1,
"1/l");
292 graph.setLegend(1, 2,
"alpha");
296 unsigned int iter = 0;
301 #ifdef USE_REAL_ROBOT
313 for (
int i=0; i<2; i++)
317 for (
int i=0; i<2; i++)
320 for (
int i=0; i<2; i++)
323 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
326 Z[i] = coef * surface[i] ;
333 s_segment.setZ1(Z[0]);
334 s_segment.setZ2(Z[1]);
358 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
361 graph.plot(0, iter, v);
362 graph.plot(1, iter, task.
getError());
365 #ifdef USE_REAL_ROBOT
374 std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s "
375 <<
vpMath::deg(v_pioneer[1]) <<
" deg/s" << std::endl;
376 std::cout <<
"Send velocity to the biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
398 #ifdef USE_REAL_ROBOT
399 std::cout <<
"Ending robot thread..." << std::endl;
400 pioneer.stopRunning();
403 pioneer.waitForRunExit();
416 std::cout <<
"ViSP is not able to control the Pioneer robot" << std::endl;