 |
Visual Servoing Platform
version 3.3.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_PUGIXML
106 if (!intrinsic_file.empty() && !camera_name.empty())
109 std::cout <<
"cam:\n" << cam << std::endl;
110 std::cout <<
"tagFamily: " << tagFamily << std::endl;
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);
174 std::stringstream ss;
175 ss <<
"Detection time: " << t <<
" ms";
186 serial->
write(
"LED_RING=2,0,10,0\n");
189 X = cMo_vec[0][0][3];
190 Y = cMo_vec[0][1][3];
191 Z = cMo_vec[0][2][3];
196 std::cout <<
"X: " << X <<
" Z: " << Z << std::endl;
204 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
207 double radius = 0.0325;
209 double motor_left = (-v[0] - L * v[1]) / radius;
210 double motor_right = ( v[0] - L * v[1]) / radius;
211 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
216 std::stringstream ss;
217 double rpm_left = motor_left * 30. / M_PI;
218 double rpm_right = motor_right * 30. / M_PI;
220 std::cout <<
"Send: " << ss.str() << std::endl;
222 serial->
write(ss.str());
228 serial->
write(
"LED_RING=2,10,0,0\n");
231 serial->
write(
"MOTOR_RPM=0,-0\n");
237 if (display_on && save_image) {
246 serial->
write(
"LED_RING=0,0,0,0\n");
249 std::cout <<
"Benchmark computation time" << std::endl;
250 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
260 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
262 serial->
write(
"LED_RING=1,10,0,0\n");
270 #ifndef VISP_HAVE_APRILTAG
271 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
273 #ifndef VISP_HAVE_V4L2
274 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
276 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...
AprilTag 36h11 pattern (recommended)
static double getMedian(const std::vector< double > &v)
void setDevice(const std::string &devname)
void setAprilTagNbThreads(int nThreads)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
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 setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
size_t getNbObjects() const
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 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))
bool detect(const vpImage< unsigned char > &I)
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)
void setAprilTagQuadDecimate(float quadDecimate)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void set_XYZ(double X, double Y, double Z)
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
Implementation of a rotation matrix and operations on such kind of matrices.
Adaptive gain computation.
unsigned int getHeight() const
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)
void buildFrom(const vpPoint &p)
static const vpColor none
const char * getMessage(void) const
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static int round(double x)
vpColVector computeControlLaw()
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.
Generic functions for unicycle mobile robots.
unsigned int getWidth() const