40 #include <visp3/core/vpConfig.h>
42 #include <visp3/robot/vpRobotPioneer.h>
43 #include <visp3/blob/vpDot2.h>
44 #include <visp3/core/vpCameraParameters.h>
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpImage.h>
47 #include <visp3/core/vpVelocityTwistMatrix.h>
48 #include <visp3/gui/vpDisplayGDI.h>
49 #include <visp3/gui/vpPlot.h>
50 #include <visp3/robot/vpPioneerPan.h>
51 #include <visp3/robot/vpRobotBiclops.h>
52 #include <visp3/sensor/vp1394CMUGrabber.h>
53 #include <visp3/sensor/vp1394TwoGrabber.h>
54 #include <visp3/sensor/vpV4l2Grabber.h>
55 #include <visp3/visual_features/vpFeatureBuilder.h>
56 #include <visp3/visual_features/vpFeatureSegment.h>
57 #include <visp3/vs/vpServo.h>
58 #include <visp3/gui/vpDisplayX.h>
60 #define USE_REAL_ROBOT
83 #if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
84 int main(
int argc,
char **argv)
86 #if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
87 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
95 double coef = 1.2 / 13.0;
98 bool normalized =
true;
108 #ifdef USE_REAL_ROBOT
133 ArArgumentParser parser(&argc, argv);
134 parser.loadDefaultArguments();
138 ArRobotConnector robotConnector(&parser, &pioneer);
139 if (!robotConnector.connectRobot()) {
140 ArLog::log(ArLog::Terse,
"Could not connect to the pioneer robot.");
141 if (parser.checkHelpAndWarnUnparsed()) {
146 if (!Aria::parseArgs()) {
160 std::cout <<
"Pioneer robot connected" << std::endl;
171 #if defined(VISP_HAVE_V4L2)
181 #elif defined(VISP_HAVE_DC1394)
189 #elif defined(VISP_HAVE_CMU1394)
204 #if defined(VISP_HAVE_X11)
206 #elif defined(VISP_HAVE_GDI)
214 for (
int i = 0; i < 2; i++) {
234 std::cout <<
"cVe: \n" << cVe << std::endl;
243 std::cout <<
"eJe: \n" << eJe << std::endl;
252 for (
int i = 0; i < 2; i++) {
261 for (
int i = 0; i < 2; i++) {
264 surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
267 Z[i] = coef * surface[i];
272 s_segment_d(normalized);
274 s_segment.setZ1(Z[0]);
275 s_segment.setZ2(Z[1]);
278 s_segment.setZ1(P[0].get_Z());
279 s_segment.setZ2(P[1].get_Z());
286 vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
290 graph.initGraph(0, 3);
291 graph.initGraph(1, 3);
292 graph.setTitle(0,
"Velocities");
293 graph.setTitle(1,
"Error s-s*");
294 graph.setLegend(0, 0,
"vx");
295 graph.setLegend(0, 1,
"wz");
296 graph.setLegend(0, 2,
"w_pan");
297 graph.setLegend(1, 0,
"xm/l");
298 graph.setLegend(1, 1,
"1/l");
299 graph.setLegend(1, 2,
"alpha");
305 unsigned int iter = 0;
307 #ifdef USE_REAL_ROBOT
319 for (
int i = 0; i < 2; i++)
323 for (
int i = 0; i < 2; i++)
326 for (
int i = 0; i < 2; i++) {
329 surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
332 Z[i] = coef * surface[i];
340 s_segment.setZ1(Z[0]);
341 s_segment.setZ2(Z[1]);
367 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
370 graph.plot(0, iter, v);
371 graph.plot(1, iter, task.
getError());
374 #ifdef USE_REAL_ROBOT
383 std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s " <<
vpMath::deg(v_pioneer[1])
384 <<
" deg/s" << std::endl;
385 std::cout <<
"Send velocity to the biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
406 #ifdef USE_REAL_ROBOT
407 std::cout <<
"Ending robot thread..." << std::endl;
408 pioneer.stopRunning();
411 pioneer.waitForRunExit();
418 std::cout <<
"Catch an exception: " << e << std::endl;
427 std::cout <<
"ViSP is not able to control the Pioneer robot" << std::endl;
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setVideoMode(unsigned long format, unsigned long mode)
void acquire(vpImage< unsigned char > &I)
void setFramerate(unsigned long fps)
void open(vpImage< unsigned char > &I)
Class for firewire ieee1394 video devices using libdc1394-2.x api.
@ vpVIDEO_MODE_640x480_MONO8
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void setGraphics(bool activate)
void setGrayLevelPrecision(const double &grayLevelPrecision)
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
void setComputeMoments(bool activate)
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
error that can be emited by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
static unsigned int selectAlpha()
static unsigned int selectXc()
static unsigned int selectL()
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Generic functions for Pioneer mobile robots equiped with a pan head.
void set_eJe(double q_pan)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
void setWorldCoordinates(double oX, double oY, double oZ)
Interface for the biclops, pan, tilt head control.
Interface for Pioneer mobile robots based on Aria 3rd party library.
void useSonar(bool usage)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
unsigned int getTaskRank() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
vpVelocityTwistMatrix get_cVe() const
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setFramerate(vpV4l2FramerateType framerate)
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void open(vpImage< unsigned char > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)