ViSP  3.0.0
servoPioneerPanSegment3D.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * IBVS on Pioneer P3DX mobile platform
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 #include <iostream>
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #include <visp3/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
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>
58 
59 #define USE_REAL_ROBOT
60 #define USE_PLOTTER
61 #undef VISP_HAVE_V4L2 // To use a firewire camera
62 
80 #if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
81 int main(int argc, char **argv)
82 {
83 #if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
84 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
85  try {
86  vpImage<unsigned char> I; // Create a gray level image container
87  double lambda = 0.1;
88  // Scale parameter used to estimate the depth Z of the blob from its surface
89  //double coef = 0.9/14.85; // At 0.9m, the blob has a surface of 14.85 (Logitec sphere)
90  double coef = 1.2/13.0; // At 1m, the blob has a surface of 11.3 (AVT Pike 032C)
91  double L = 0.21; // 3D horizontal segment length
92  double Z_d = 0.8; // Desired distance along Z between camera and segment
93  bool normalized = true; // segment normilized features are used
94 
95  // Warning: To have a non singular task of rank 3, Y_d should be different from 0 so that
96  // the optical axis doesn't intersect the horizontal segment
97  double Y_d = -.11; // Desired distance along Y between camera and segment.
98  vpColVector qm(2); // Measured head position
99  qm = 0;
100  double qm_pan = 0; // Measured pan position (tilt is not handled in that example)
101 
102 #ifdef USE_REAL_ROBOT
103  // Initialize the biclops head
104 
105  vpRobotBiclops biclops("/usr/share/BiclopsDefault.cfg");
106  biclops.setDenavitHartenbergModel(vpBiclops::DH1);
107 
108  // Move to the initial position
109  vpColVector q(2);
110 
111  q=0;
112  // q[0] = vpMath::rad(63);
113  // q[1] = vpMath::rad(12); // introduce a tilt angle to compensate camera sphere tilt so that the camera is parallel to the plane
114 
115  biclops.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
116  biclops.setPosition( vpRobot::ARTICULAR_FRAME, q );
117  //biclops.setPositioningVelocity(50);
118  biclops.getPosition(vpRobot::ARTICULAR_FRAME, qm);
119  qm_pan = qm[0];
120 
121  // Now the head will be controlled in velocity
122  biclops.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
123 
124  // Initialize the pioneer robot
125  vpRobotPioneer pioneer;
126  ArArgumentParser parser(&argc, argv);
127  parser.loadDefaultArguments();
128 
129  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
130  // and then loads parameter files for this robot.
131  ArRobotConnector robotConnector(&parser, &pioneer);
132  if(!robotConnector.connectRobot())
133  {
134  ArLog::log(ArLog::Terse, "Could not connect to the pioneer robot.");
135  if(parser.checkHelpAndWarnUnparsed())
136  {
137  Aria::logOptions();
138  Aria::exit(1);
139  }
140  }
141  if (!Aria::parseArgs())
142  {
143  Aria::logOptions();
144  Aria::shutdown();
145  return false;
146  }
147 
148  pioneer.useSonar(false); // disable the sonar device usage
149 
150  // Wait 3 sec to be sure that the low level Aria thread used to control
151  // the robot is started. Without this delay we experienced a delay (arround 2.2 sec)
152  // between the velocity send to the robot and the velocity that is really applied
153  // to the wheels.
154  sleep(3);
155 
156  std::cout << "Pioneer robot connected" << std::endl;
157 #endif
158 
159  vpPioneerPan robot_pan; // Generic robot that computes the velocities for the pioneer and the biclops head
160 
161  // Camera parameters. In this experiment we don't need a precise calibration of the camera
162  vpCameraParameters cam;
163 
164  // Create the camera framegrabber
165 #if defined(VISP_HAVE_V4L2)
166  // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
167  vpV4l2Grabber g;
168  g.setScale(1);
169  g.setInput(0);
170  g.setDevice("/dev/video1");
171  g.open(I);
172  // Logitec sphere parameters
173  cam.initPersProjWithoutDistortion(558, 555, 312, 210);
174 #elif defined(VISP_HAVE_DC1394)
175  // Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
176  vp1394TwoGrabber g(false);
179  // AVT Pike 032C parameters
180  cam.initPersProjWithoutDistortion(800, 795, 320, 216);
181 #elif defined(VISP_HAVE_CMU1394)
182  // Create a grabber based on CMU 1394 third party lib (for firewire cameras under windows)
184  g.setVideoMode(0, 5); // 640x480 MONO8
185  g.setFramerate(4); // 30 Hz
186  g.open(I);
187  // AVT Pike 032C parameters
188  cam.initPersProjWithoutDistortion(800, 795, 320, 216);
189 #endif
190 
191  // Acquire an image from the grabber
192  g.acquire(I);
193 
194  // Create an image viewer
195 #if defined(VISP_HAVE_X11)
196  vpDisplayX d(I, 10, 10, "Current frame");
197 #elif defined(VISP_HAVE_GDI)
198  vpDisplayGDI d(I, 10, 10, "Current frame");
199 #endif
201  vpDisplay::flush(I);
202 
203  // The 3D segment consists in two horizontal dots
204  vpDot2 dot[2];
205  for (int i=0; i <2; i++)
206  {
207  dot[i].setGraphics(true);
208  dot[i].setComputeMoments(true);
209  dot[i].setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
210  dot[i].setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation
211  dot[i].setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner and outside points with bad gray level
212  dot[i].initTracking(I);
213  vpDisplay::flush(I);
214  }
215 
216  vpServo task;
219  task.setLambda(lambda) ;
220  vpVelocityTwistMatrix cVe ; // keep to identity
221  cVe = robot_pan.get_cVe() ;
222  task.set_cVe(cVe) ;
223 
224  std::cout << "cVe: \n" << cVe << std::endl;
225 
226  vpMatrix eJe;
227 
228  // Update the robot jacobian that depends on the pan position
229  robot_pan.set_eJe(qm_pan);
230  // Get the robot jacobian
231  eJe = robot_pan.get_eJe() ;
232  task.set_eJe(eJe) ;
233  std::cout << "eJe: \n" << eJe << std::endl;
234 
235  // Define a 3D horizontal segment an its cordinates in the image plane
236  vpPoint P[2];
237  P[0].setWorldCoordinates(-L/2, 0, 0);
238  P[1].setWorldCoordinates( L/2, 0, 0);
239  // Define the desired camera position
240  vpHomogeneousMatrix cMo(0, Y_d, Z_d, 0, 0, 0); // Here we are in front of the segment
241  for (int i=0; i <2; i++)
242  {
243  P[i].changeFrame(cMo);
244  P[i].project(); // Here the x,y parameters obtained by perspective projection are computed
245  }
246 
247  // Estimate the depth of the segment extremity points
248  double surface[2];
249  double Z[2]; // Depth of the segment points
250  for (int i=0; i<2; i++)
251  {
252  // Surface of the blob estimated from the image moment m00 and converted in meters
253  surface[i] = 1./sqrt(dot[i].m00/(cam.get_px()*cam.get_py()));
254 
255  // Initial depth of the blob
256  Z[i] = coef * surface[i] ;
257  }
258 
259  // Use here a feature segment builder
260  vpFeatureSegment s_segment(normalized), s_segment_d(normalized); // From the segment feature we use only alpha
261  vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
262  s_segment.setZ1(Z[0]);
263  s_segment.setZ2(Z[1]);
264  // Set the desired feature
265  vpFeatureBuilder::create(s_segment_d, P[0], P[1]);
266  s_segment.setZ1( P[0].get_Z() ); // Desired depth
267  s_segment.setZ2( P[1].get_Z() );
268 
269  task.addFeature(s_segment, s_segment_d,
273 
274 #ifdef USE_PLOTTER
275  //Create a window (500 by 500) at position (700, 10) with two graphics
276  vpPlot graph(2, 500, 500, 700, 10, "Curves...");
277 
278  //The first graphic contains 3 curve and the second graphic contains 3 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");
289 #endif
290 
291  vpColVector v; // vz, wx
292  unsigned int iter = 0;
293  try
294  {
295  while(1)
296  {
297 #ifdef USE_REAL_ROBOT
298  // Get the new pan position
299  biclops.getPosition(vpRobot::ARTICULAR_FRAME, qm);
300 #endif
301  qm_pan = qm[0];
302 
303  // Acquire a new image
304  g.acquire(I);
305  // Set the image as background of the viewer
307 
308  // Display the desired position of the segment
309  for (int i=0; i<2; i++)
310  P[i].display(I, cam, vpColor::red, 3);
311 
312  // Does the blob tracking
313  for (int i=0; i<2; i++)
314  dot[i].track(I);
315 
316  for (int i=0; i<2; i++)
317  {
318  // Surface of the blob estimated from the image moment m00 and converted in meters
319  surface[i] = 1./sqrt(dot[i].m00/(cam.get_px()*cam.get_py()));
320 
321  // Initial depth of the blob
322  Z[i] = coef * surface[i] ;
323  }
324 
325  // Update the features
326  vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
327  // Update the depth of the point. Useful only if current interaction matrix is used
328  // when task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) is set
329  s_segment.setZ1(Z[0]);
330  s_segment.setZ2(Z[1]);
331 
332  robot_pan.get_cVe(cVe);
333  task.set_cVe(cVe);
334 
335  // Update the robot jacobian that depends on the pan position
336  robot_pan.set_eJe(qm_pan);
337  // Get the robot jacobian
338  eJe = robot_pan.get_eJe();
339  // Update the jacobian that will be used to compute the control law
340  task.set_eJe(eJe);
341 
342  // Compute the control law. Velocities are computed in the mobile robot reference frame
343  v = task.computeControlLaw();
344 
345  // std::cout << "-----" << std::endl;
346  // std::cout << "v: " << v.t() << std::endl;
347  // std::cout << "error: " << task.getError().t() << std::endl;
348  // std::cout << "L:\n " << task.getInteractionMatrix() << std::endl;
349  // std::cout << "eJe:\n " << task.get_eJe() << std::endl;
350  // std::cout << "cVe:\n " << task.get_cVe() << std::endl;
351  // std::cout << "L_cVe_eJe:\n" << task.getInteractionMatrix() * task.get_cVe() * task.get_eJe() << std::endl;
352  // task.print() ;
353  if (task.getTaskRank() != 3)
354  std::cout << "Warning: task is of rank " << task.getTaskRank() << std::endl;
355 
356 #ifdef USE_PLOTTER
357  graph.plot(0, iter, v); // plot velocities applied to the robot
358  graph.plot(1, iter, task.getError()); // plot error vector
359 #endif
360 
361 #ifdef USE_REAL_ROBOT
362  // Send the velocity to the robot
363  vpColVector v_pioneer(2); // vx, wz
364  v_pioneer[0] = v[0];
365  v_pioneer[1] = v[1];
366  vpColVector v_biclops(2); // qdot pan and tilt
367  v_biclops[0] = v[2];
368  v_biclops[1] = 0;
369 
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;
373 
374  pioneer.setVelocity(vpRobot::REFERENCE_FRAME, v_pioneer);
375  biclops.setVelocity(vpRobot::ARTICULAR_FRAME, v_biclops) ;
376 #endif
377 
378  // Draw a vertical line which corresponds to the desired x coordinate of the dot cog
379  vpDisplay::displayLine(I, 0, cam.get_u0(), 479, cam.get_u0(), vpColor::red);
380  vpDisplay::flush(I);
381 
382  // A click in the viewer to exit
383  if ( vpDisplay::getClick(I, false) )
384  break;
385 
386  iter ++;
387  //break;
388  }
389  }
390  catch(...)
391  {
392  }
393 
394 #ifdef USE_REAL_ROBOT
395  std::cout << "Ending robot thread..." << std::endl;
396  pioneer.stopRunning();
397 
398  // wait for the thread to stop
399  pioneer.waitForRunExit();
400 #endif
401 
402  // Kill the servo task
403  task.print() ;
404  task.kill();
405  }
406  catch(vpException e) {
407  std::cout << "Catch an exception: " << e << std::endl;
408  return 1;
409  }
410 #endif
411 #endif
412 }
413 #else
414 int main()
415 {
416  std::cout << "ViSP is not able to control the Pioneer robot" << std::endl;
417  return 0;
418 }
419 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
double get_u0() const
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).
Definition: vpDisplayGDI.h:128
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
Definition: vpDot2.h:285
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:459
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:85
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Define the X11 console to display images.
Definition: vpDisplayX.h:148
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:446
void setDevice(const std::string &devname)
Initialize the position controller.
Definition: vpRobot.h:69
error that can be emited by ViSP classes.
Definition: vpException.h:73
Interface for Pioneer mobile robots based on Aria 3rd party library.
double get_py() const
Generic functions for Pioneer mobile robots equiped with a pan head.
Definition: vpPioneerPan.h:95
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:124
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
static const vpColor red
Definition: vpColor.h:163
Class that defines what is a point.
Definition: vpPoint.h:59
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void setGrayLevelPrecision(const double &grayLevelPrecision)
Definition: vpDot2.cpp:784
void kill()
Definition: vpServo.cpp:186
Initialize the velocity controller.
Definition: vpRobot.h:68
vpColVector getError() const
Definition: vpServo.h:271
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setFramerate(unsigned long fps)
vpColVector computeControlLaw()
Definition: vpServo.cpp:899
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)
Definition: vpDisplay.cpp:206
vpMatrix get_eJe() const
Definition: vpUnicycle.h:111
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:390
void setComputeMoments(const bool activate)
Definition: vpDot2.h:271
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.
double get_px() const
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
Definition: vpDot2.cpp:859
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:519
Class for the Video4Linux2 video device.
static double deg(double rad)
Definition: vpMath.h:97
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:434
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:262
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:248
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:113
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)
Definition: vpPoint.cpp:247
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:217
void set_eJe(double q_pan)
Definition: vpPioneerPan.h:140
static unsigned int selectXc()
unsigned int getTaskRank() const
Definition: vpServo.cpp:1794
void setGraphics(const bool activate)
Definition: vpDot2.h:309
void setFramerate(vpV4l2FramerateType framerate)