4 #include <visp/vpCameraParameters.h> 5 #include <visp/vpDisplayX.h> 6 #include <visp/vpDot2.h> 7 #include <visp/vpFeatureBuilder.h> 8 #include <visp/vpFeatureDepth.h> 9 #include <visp/vpFeaturePoint.h> 10 #include <visp/vpHomogeneousMatrix.h> 11 #include <visp/vpImage.h> 12 #include <visp/vpImageConvert.h> 13 #include <visp/vpServo.h> 14 #include <visp/vpVelocityTwistMatrix.h> 19 #if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_X11) 20 # define TEST_COULD_BE_ACHIEVED 43 #ifdef TEST_COULD_BE_ACHIEVED 44 int main(
int argc,
char **argv)
47 vpImage<unsigned char> I;
50 double coef = 1./6.77;
54 robot.
init(argc, argv);
60 vpTime::sleepMs(3000);
62 std::cout <<
"Robot connected" << std::endl;
65 vpCameraParameters cam;
75 cam.initPersProjWithoutDistortion(600,600,I.getWidth()/2, I.getHeight()/2);
80 vpDisplayX
d(I, 10, 10,
"Current frame");
81 vpDisplay::display(I);
86 dot.setGraphics(
true);
87 dot.setComputeMoments(
true);
88 dot.setEllipsoidShapePrecision(0.);
89 dot.setGrayLevelPrecision(0.9);
90 dot.setEllipsoidBadPointsPercentage(0.5);
95 task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
96 task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
97 task.setLambda(lambda) ;
98 vpVelocityTwistMatrix cVe ;
99 cVe = robot.get_cVe() ;
102 std::cout <<
"cVe: \n" << cVe << std::endl;
107 std::cout <<
"eJe: \n" << eJe << std::endl;
110 vpFeaturePoint s_x, s_xd;
113 vpFeatureBuilder::create(s_x, cam, dot);
116 s_xd.buildFrom(0, 0, depth);
119 task.addFeature(s_x, s_xd) ;
122 vpFeatureDepth s_Z, s_Zd;
124 double surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
131 std::cout <<
"Z " << Z << std::endl;
132 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z , 0);
133 s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd , 0);
136 task.addFeature(s_Z, s_Zd) ;
146 vpDisplay::display(I);
151 vpFeatureBuilder::create(s_x, cam, dot);
154 surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
156 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
165 v = task.computeControlLaw() ;
167 std::cout <<
"Send velocity to the pionner: " << v[0] <<
" m/s " 168 << vpMath::deg(v[1]) <<
" deg/s" << std::endl;
174 vpDisplay::displayLine(I, 0, 320, 479, 320, vpColor::red);
178 if ( vpDisplay::getClick(I,
false) )
182 std::cout <<
"Ending robot thread..." << std::endl;
188 catch(vpException e) {
189 std::cout <<
"Catch an exception: " << e << std::endl;
196 std::cout <<
"You don't have the right 3rd party libraries to run this example..." << std::endl;
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Class for cameras video capture for ROS cameras.
void setCmdVelTopic(std::string topic_name)
void setRectify(bool rectify)
void init()
basic initialization
void setCameraInfoTopic(std::string topic_name)
void open(int argc, char **argv)
bool getCameraInfo(vpCameraParameters &cam)
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
class for Camera video capture for ROS middleware.
void get_eJe(vpMatrix &eJe)
Interface for Pioneer mobile robots based on ROS.
void setImageTopic(std::string topic_name)
void acquire(vpImage< unsigned char > &I)