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;
96 bool normalized =
true;
105 #ifdef USE_REAL_ROBOT
129 ArArgumentParser parser(&argc, argv);
130 parser.loadDefaultArguments();
134 ArRobotConnector robotConnector(&parser, &pioneer);
135 if(!robotConnector.connectRobot())
137 ArLog::log(ArLog::Terse,
"Could not connect to the pioneer robot.");
138 if(parser.checkHelpAndWarnUnparsed())
144 if (!Aria::parseArgs())
159 std::cout <<
"Pioneer robot connected" << std::endl;
168 #if defined(VISP_HAVE_V4L2)
177 #elif defined(VISP_HAVE_DC1394_2)
184 #elif defined(VISP_HAVE_CMU1394)
198 #if defined(VISP_HAVE_X11)
200 #elif defined(VISP_HAVE_GDI)
208 for (
int i=0; i <2; i++)
227 std::cout <<
"cVe: \n" << cVe << std::endl;
236 std::cout <<
"eJe: \n" << eJe << std::endl;
244 for (
int i=0; i <2; i++)
253 for (
int i=0; i<2; i++)
256 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
259 Z[i] = coef * surface[i] ;
265 s_segment.setZ1(Z[0]);
266 s_segment.setZ2(Z[1]);
269 s_segment.setZ1( P[0].get_Z() );
270 s_segment.setZ2( P[1].get_Z() );
279 vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
282 graph.initGraph(0,3);
283 graph.initGraph(1,3);
284 graph.setTitle(0,
"Velocities");
285 graph.setTitle(1,
"Error s-s*");
286 graph.setLegend(0, 0,
"vx");
287 graph.setLegend(0, 1,
"wz");
288 graph.setLegend(0, 2,
"w_pan");
289 graph.setLegend(1, 0,
"xm/l");
290 graph.setLegend(1, 1,
"1/l");
291 graph.setLegend(1, 2,
"alpha");
295 unsigned int iter = 0;
300 #ifdef USE_REAL_ROBOT
312 for (
int i=0; i<2; i++)
316 for (
int i=0; i<2; i++)
319 for (
int i=0; i<2; i++)
322 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
325 Z[i] = coef * surface[i] ;
332 s_segment.setZ1(Z[0]);
333 s_segment.setZ2(Z[1]);
357 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
360 graph.plot(0, iter, v);
361 graph.plot(1, iter, task.
getError());
364 #ifdef USE_REAL_ROBOT
373 std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s "
374 <<
vpMath::deg(v_pioneer[1]) <<
" deg/s" << std::endl;
375 std::cout <<
"Send velocity to the biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
397 #ifdef USE_REAL_ROBOT
398 std::cout <<
"Ending robot thread..." << std::endl;
399 pioneer.stopRunning();
402 pioneer.waitForRunExit();
415 std::cout <<
"ViSP is not able to control the Pioneer robot" << std::endl;