Go to the documentation of this file.00001
00002 #include <iostream>
00003
00004 #include <visp/vpCameraParameters.h>
00005 #include <visp/vpDisplayX.h>
00006 #include <visp/vpDot2.h>
00007 #include <visp/vpFeatureBuilder.h>
00008 #include <visp/vpFeatureDepth.h>
00009 #include <visp/vpFeaturePoint.h>
00010 #include <visp/vpHomogeneousMatrix.h>
00011 #include <visp/vpImage.h>
00012 #include <visp/vpImageConvert.h>
00013 #include <visp/vpServo.h>
00014 #include <visp/vpVelocityTwistMatrix.h>
00015
00016 #include <visp_ros/vpROSGrabber.h>
00017 #include <visp_ros/vpROSRobotPioneer.h>
00018
00019 #if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_X11)
00020 # define TEST_COULD_BE_ACHIEVED
00021 #endif
00022
00023
00043 #ifdef TEST_COULD_BE_ACHIEVED
00044 int main(int argc, char **argv)
00045 {
00046 try {
00047 vpImage<unsigned char> I;
00048 double depth = 1.;
00049 double lambda = 0.6;
00050 double coef = 1./6.77;
00051
00052 vpROSRobotPioneer robot;
00053 robot.setCmdVelTopic("/RosAria/cmd_vel");
00054 robot.init(argc, argv);
00055
00056
00057
00058
00059
00060 vpTime::sleepMs(3000);
00061
00062 std::cout << "Robot connected" << std::endl;
00063
00064
00065 vpCameraParameters cam;
00066
00067
00068 vpROSGrabber g;
00069 g.setCameraInfoTopic("/camera/camera_info");
00070 g.setImageTopic("/camera/image_raw");
00071 g.setRectify(true);
00072 g.open(argc, argv);
00073
00074 if (g.getCameraInfo(cam) == false)
00075 cam.initPersProjWithoutDistortion(600,600,I.getWidth()/2, I.getHeight()/2);
00076
00077 g.acquire(I);
00078
00079
00080 vpDisplayX d(I, 10, 10, "Current frame");
00081 vpDisplay::display(I);
00082 vpDisplay::flush(I);
00083
00084
00085 vpDot2 dot;
00086 dot.setGraphics(true);
00087 dot.setComputeMoments(true);
00088 dot.setEllipsoidShapePrecision(0.);
00089 dot.setGrayLevelPrecision(0.9);
00090 dot.setEllipsoidBadPointsPercentage(0.5);
00091 dot.initTracking(I);
00092 vpDisplay::flush(I);
00093
00094 vpServo task;
00095 task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
00096 task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
00097 task.setLambda(lambda) ;
00098 vpVelocityTwistMatrix cVe ;
00099 cVe = robot.get_cVe() ;
00100 task.set_cVe(cVe) ;
00101
00102 std::cout << "cVe: \n" << cVe << std::endl;
00103
00104 vpMatrix eJe;
00105 robot.get_eJe(eJe) ;
00106 task.set_eJe(eJe) ;
00107 std::cout << "eJe: \n" << eJe << std::endl;
00108
00109
00110 vpFeaturePoint s_x, s_xd;
00111
00112
00113 vpFeatureBuilder::create(s_x, cam, dot);
00114
00115
00116 s_xd.buildFrom(0, 0, depth);
00117
00118
00119 task.addFeature(s_x, s_xd) ;
00120
00121
00122 vpFeatureDepth s_Z, s_Zd;
00123
00124 double surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
00125 double Z, Zd;
00126
00127 Z = coef * surface ;
00128
00129 Zd = Z;
00130
00131 std::cout << "Z " << Z << std::endl;
00132 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z , 0);
00133 s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd , 0);
00134
00135
00136 task.addFeature(s_Z, s_Zd) ;
00137
00138 vpColVector v;
00139
00140 while(1)
00141 {
00142
00143 g.acquire(I);
00144
00145
00146 vpDisplay::display(I);
00147
00148
00149 dot.track(I);
00150
00151 vpFeatureBuilder::create(s_x, cam, dot);
00152
00153
00154 surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
00155 Z = coef * surface ;
00156 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
00157
00158 robot.get_cVe(cVe) ;
00159 task.set_cVe(cVe) ;
00160
00161 robot.get_eJe(eJe) ;
00162 task.set_eJe(eJe) ;
00163
00164
00165 v = task.computeControlLaw() ;
00166
00167 std::cout << "Send velocity to the pionner: " << v[0] << " m/s "
00168 << vpMath::deg(v[1]) << " deg/s" << std::endl;
00169
00170
00171 robot.setVelocity(vpRobot::REFERENCE_FRAME, v);
00172
00173
00174 vpDisplay::displayLine(I, 0, 320, 479, 320, vpColor::red);
00175 vpDisplay::flush(I);
00176
00177
00178 if ( vpDisplay::getClick(I, false) )
00179 break;
00180 }
00181
00182 std::cout << "Ending robot thread..." << std::endl;
00183
00184
00185 task.print() ;
00186 task.kill();
00187 }
00188 catch(vpException e) {
00189 std::cout << "Catch an exception: " << e << std::endl;
00190 return 1;
00191 }
00192 }
00193 #else
00194 int main()
00195 {
00196 std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl;
00197 }
00198 #endif