 |
Visual Servoing Platform
version 3.2.0
|
1 #include <visp3/core/vpXmlParserCamera.h>
3 #include <visp3/core/vpSerial.h>
4 #include <visp3/detection/vpDetectorAprilTag.h>
5 #include <visp3/gui/vpDisplayX.h>
6 #include <visp3/sensor/vpV4l2Grabber.h>
7 #include <visp3/io/vpImageIo.h>
8 #include <visp3/visual_features/vpFeaturePoint3D.h>
9 #include <visp3/vs/vpServo.h>
10 #include <visp3/robot/vpUnicycle.h>
12 int main(
int argc,
const char **argv)
14 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
18 double tagSize = 0.065;
19 float quad_decimate = 4.0;
21 std::string intrinsic_file =
"";
22 std::string camera_name =
"";
23 bool display_tag =
false;
24 bool display_on =
false;
25 bool serial_off =
false;
26 bool save_image =
false;
28 for (
int i = 1; i < argc; i++) {
29 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
30 tagSize = std::atof(argv[i + 1]);
31 }
else if (std::string(argv[i]) ==
"--input" && i + 1 < argc) {
32 device = std::atoi(argv[i + 1]);
33 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
34 quad_decimate = (float)atof(argv[i + 1]);
35 }
else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
36 nThreads = std::atoi(argv[i + 1]);
37 }
else if (std::string(argv[i]) ==
"--intrinsic" && i + 1 < argc) {
38 intrinsic_file = std::string(argv[i + 1]);
39 }
else if (std::string(argv[i]) ==
"--camera_name" && i + 1 < argc) {
40 camera_name = std::string(argv[i + 1]);
41 }
else if (std::string(argv[i]) ==
"--display_tag") {
43 #if defined(VISP_HAVE_X11)
44 }
else if (std::string(argv[i]) ==
"--display_on") {
46 }
else if (std::string(argv[i]) ==
"--save_image") {
49 }
else if (std::string(argv[i]) ==
"--serial_off") {
51 }
else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
53 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
54 std::cout <<
"Usage: " << argv[0]
55 <<
" [--input <camera input>] [--tag_size <tag_size in m>]"
56 " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
57 " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
58 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT,"
59 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
61 #if defined(VISP_HAVE_X11)
62 std::cout <<
" [--display_on] [--save_image]";
64 std::cout <<
" [--serial_off] [--help]" << std::endl;
78 serial =
new vpSerial(
"/dev/ttyAMA0", 115200);
80 serial->
write(
"LED_RING=0,0,0,0\n");
81 serial->
write(
"LED_RING=1,0,10,0\n");
88 std::ostringstream device_name;
89 device_name <<
"/dev/video" << device;
104 #ifdef VISP_HAVE_XML2
106 if (!intrinsic_file.empty() && !camera_name.empty())
109 std::cout <<
"cam:\n" << cam << std::endl;
110 std::cout <<
"tagFamily: " << tagFamily << std::endl;
114 detector.setAprilTagQuadDecimate(quad_decimate);
115 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
116 detector.setAprilTagNbThreads(nThreads);
117 detector.setDisplayTag(display_tag);
131 cRe[0][0] = 0; cRe[0][1] = -1; cRe[0][2] = 0;
132 cRe[1][0] = 0; cRe[1][1] = 0; cRe[1][2] = -1;
133 cRe[2][0] = 1; cRe[2][1] = 0; cRe[2][2] = 0;
140 eJe[0][0] = eJe[5][1] = 1.0;
142 std::cout <<
"eJe: \n" << eJe << std::endl;
146 double X = 0, Y = 0, Z = Z_d;
160 std::vector<double> time_vec;
167 std::vector<vpHomogeneousMatrix> cMo_vec;
168 detector.detect(I, tagSize, cam, cMo_vec);
171 time_vec.push_back(t);
173 std::stringstream ss;
174 ss <<
"Detection time: " << t <<
" ms";
177 if (detector.getNbObjects() == 1) {
184 serial->
write(
"LED_RING=2,0,10,0\n");
187 double X = cMo_vec[0][0][3];
188 double Y = cMo_vec[0][1][3];
189 double Z = cMo_vec[0][2][3];
194 std::cout <<
"X: " << X <<
" Z: " << Z << std::endl;
202 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
205 double radius = 0.0325;
207 double motor_left = (-v[0] - L * v[1]) / radius;
208 double motor_right = ( v[0] - L * v[1]) / radius;
209 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
214 std::stringstream ss;
215 double rpm_left = motor_left * 30. / M_PI;
216 double rpm_right = motor_right * 30. / M_PI;
218 std::cout <<
"Send: " << ss.str() << std::endl;
220 serial->
write(ss.str());
226 serial->
write(
"LED_RING=2,10,0,0\n");
229 serial->
write(
"MOTOR_RPM=0,-0\n");
235 if (display_on && save_image) {
244 serial->
write(
"LED_RING=0,0,0,0\n");
247 std::cout <<
"Benchmark computation time" << std::endl;
248 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
258 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
260 serial->
write(
"LED_RING=1,10,0,0\n");
268 #ifndef VISP_HAVE_APRILTAG
269 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
271 #ifndef VISP_HAVE_V4L2
272 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
274 std::cout <<
"Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
Class that defines the 3D point visual feature.
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
static double getMedian(const std::vector< double > &v)
void setDevice(const std::string &devname)
Generic class defining intrinsic camera parameters.
XML parser to load and save intrinsic camera parameters.
void set_eJe(const vpMatrix &eJe_)
void write(const std::string &s)
static double deg(double rad)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Class that consider the case of a translation vector.
static double getMean(const std::vector< double > &v)
void acquire(vpImage< unsigned char > &I)
static int round(const double x)
static unsigned int selectX()
Implementation of column vector and the associated operations.
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
VISP_EXPORT double measureTimeMs()
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
static void write(const vpImage< unsigned char > &I, const std::string &filename)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void display(const vpImage< unsigned char > &I)
static double getStdev(const std::vector< double > &v, const bool useBesselCorrection=false)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of a rotation matrix and operations on such kind of matrices.
Adaptive gain computation.
unsigned int getHeight() const
void set_XYZ(const double X, const double Y, const double Z)
void buildFrom(const vpPoint &p)
static const vpColor none
const char * getMessage(void) const
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines generic functionnalities for display.
static unsigned int selectZ()
error that can be emited by ViSP classes.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
Generic functions for unicycle mobile robots.
unsigned int getWidth() const