37 #include <visp3/core/vpConfig.h>
39 #include <visp3/blob/vpDot2.h>
40 #include <visp3/core/vpCameraParameters.h>
41 #include <visp3/core/vpHomogeneousMatrix.h>
42 #include <visp3/core/vpImage.h>
43 #include <visp3/core/vpVelocityTwistMatrix.h>
44 #include <visp3/gui/vpDisplayGDI.h>
45 #include <visp3/gui/vpDisplayX.h>
46 #include <visp3/gui/vpPlot.h>
47 #include <visp3/robot/vpPioneerPan.h>
48 #include <visp3/robot/vpRobotBiclops.h>
49 #include <visp3/robot/vpRobotPioneer.h>
50 #include <visp3/sensor/vp1394CMUGrabber.h>
51 #include <visp3/sensor/vp1394TwoGrabber.h>
52 #include <visp3/sensor/vpV4l2Grabber.h>
53 #include <visp3/visual_features/vpFeatureBuilder.h>
54 #include <visp3/visual_features/vpFeatureSegment.h>
55 #include <visp3/vs/vpServo.h>
57 #define USE_REAL_ROBOT
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)
92 double coef = 1.2 / 13.0;
95 bool normalized =
true;
105 #ifdef USE_REAL_ROBOT
130 ArArgumentParser parser(&argc, argv);
131 parser.loadDefaultArguments();
135 ArRobotConnector robotConnector(&parser, &pioneer);
136 if (!robotConnector.connectRobot()) {
137 ArLog::log(ArLog::Terse,
"Could not connect to the pioneer robot.");
138 if (parser.checkHelpAndWarnUnparsed()) {
143 if (!Aria::parseArgs()) {
157 std::cout <<
"Pioneer robot connected" << std::endl;
168 #if defined(VISP_HAVE_V4L2)
178 #elif defined(VISP_HAVE_DC1394)
186 #elif defined(VISP_HAVE_CMU1394)
201 #if defined(VISP_HAVE_X11)
203 #elif defined(VISP_HAVE_GDI)
211 for (
int i = 0; i < 2; i++) {
231 std::cout <<
"cVe: \n" << cVe << std::endl;
240 std::cout <<
"eJe: \n" << eJe << std::endl;
249 for (
int i = 0; i < 2; i++) {
258 for (
int i = 0; i < 2; i++) {
261 surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
264 Z[i] = coef * surface[i];
269 s_segment_d(normalized);
271 s_segment.setZ1(Z[0]);
272 s_segment.setZ2(Z[1]);
275 s_segment.setZ1(P[0].get_Z());
276 s_segment.setZ2(P[1].get_Z());
283 vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
287 graph.initGraph(0, 3);
288 graph.initGraph(1, 3);
289 graph.setTitle(0,
"Velocities");
290 graph.setTitle(1,
"Error s-s*");
291 graph.setLegend(0, 0,
"vx");
292 graph.setLegend(0, 1,
"wz");
293 graph.setLegend(0, 2,
"w_pan");
294 graph.setLegend(1, 0,
"xm/l");
295 graph.setLegend(1, 1,
"1/l");
296 graph.setLegend(1, 2,
"alpha");
302 unsigned int iter = 0;
304 #ifdef USE_REAL_ROBOT
316 for (
int i = 0; i < 2; i++)
320 for (
int i = 0; i < 2; i++)
323 for (
int i = 0; i < 2; i++) {
326 surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
329 Z[i] = coef * surface[i];
337 s_segment.setZ1(Z[0]);
338 s_segment.setZ2(Z[1]);
364 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
367 graph.plot(0, iter, v);
368 graph.plot(1, iter, task.
getError());
371 #ifdef USE_REAL_ROBOT
380 std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s " <<
vpMath::deg(v_pioneer[1])
381 <<
" deg/s" << std::endl;
382 std::cout <<
"Send velocity to the Biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
403 #ifdef USE_REAL_ROBOT
404 std::cout <<
"Ending robot thread..." << std::endl;
405 pioneer.stopRunning();
408 pioneer.waitForRunExit();
415 std::cout <<
"Catch an exception: " << e << std::endl;
424 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
@ DH1
First Denavit-Hartenberg representation.
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 emitted 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 equipped 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 vp_override
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) vp_override
@ 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 addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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()
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)
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.