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");
295 unsigned int iter = 0;
298 #ifdef USE_REAL_ROBOT 310 for (
int i=0; i<2; i++)
314 for (
int i=0; i<2; i++)
317 for (
int i=0; i<2; i++)
320 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
323 Z[i] = coef * surface[i] ;
330 s_segment.setZ1(Z[0]);
331 s_segment.setZ2(Z[1]);
355 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
358 graph.plot(0, iter, v);
359 graph.plot(1, iter, task.
getError());
362 #ifdef USE_REAL_ROBOT 371 std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s " 372 <<
vpMath::deg(v_pioneer[1]) <<
" deg/s" << std::endl;
373 std::cout <<
"Send velocity to the biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
395 #ifdef USE_REAL_ROBOT 396 std::cout <<
"Ending robot thread..." << std::endl;
397 pioneer.stopRunning();
400 pioneer.waitForRunExit();
408 std::cout <<
"Catch an exception: " << e << std::endl;
417 std::cout <<
"ViSP is not able to control the Pioneer robot" << std::endl;
Implementation of a matrix and operations on matrices.
vpVelocityTwistMatrix get_cVe() const
unsigned int getTaskRank() const
static unsigned int selectAlpha()
void setVideoMode(unsigned long format, unsigned long mode)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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_)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
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.
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 that is a wrapper over the Video4Linux2 (V4L2) driver.
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...
vpColVector getError() const
Class for firewire ieee1394 video devices using libdc1394-2.x api.
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
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()
void setGraphics(const bool activate)
void setFramerate(vpV4l2FramerateType framerate)