60 #include <visp3/core/vpCameraParameters.h>
61 #include <visp3/detection/vpDetectorAprilTag.h>
62 #include <visp3/gui/vpDisplayGDI.h>
63 #include <visp3/gui/vpDisplayX.h>
64 #include <visp3/gui/vpPlot.h>
65 #include <visp3/io/vpImageIo.h>
66 #include <visp3/robot/vpRobotFlirPtu.h>
67 #include <visp3/sensor/vpFlyCaptureGrabber.h>
68 #include <visp3/visual_features/vpFeatureBuilder.h>
69 #include <visp3/visual_features/vpFeaturePoint.h>
70 #include <visp3/vs/vpServo.h>
71 #include <visp3/vs/vpServoDisplay.h>
73 #if defined(VISP_HAVE_FLIR_PTU_SDK) && defined(VISP_HAVE_FLYCAPTURE) && \
74 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
76 int main(
int argc,
char **argv)
78 std::string opt_portname;
79 int opt_baudrate = 9600;
80 bool opt_network =
false;
81 std::string opt_extrinsic;
82 double opt_tag_size = 0.120;
83 double opt_constant_gain = 0.5;
86 std::cout <<
"To see how to use this example, run: " << argv[0] <<
" --help" << std::endl;
90 for (
int i = 1; i < argc; i++) {
91 if ((std::string(argv[i]) ==
"--portname" || std::string(argv[i]) ==
"-p") && (i + 1 < argc)) {
92 opt_portname = std::string(argv[i + 1]);
94 else if ((std::string(argv[i]) ==
"--baudrate" || std::string(argv[i]) ==
"-b") && (i + 1 < argc)) {
95 opt_baudrate = std::atoi(argv[i + 1]);
97 else if ((std::string(argv[i]) ==
"--network" || std::string(argv[i]) ==
"-n")) {
100 else if (std::string(argv[i]) ==
"--extrinsic" && i + 1 < argc) {
101 opt_extrinsic = std::string(argv[i + 1]);
103 else if (std::string(argv[i]) ==
"--constant-gain" || std::string(argv[i]) ==
"-g") {
104 opt_constant_gain = std::stod(argv[i + 1]);
107 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
108 std::cout <<
"SYNOPSIS" << std::endl
109 <<
" " << argv[0] <<
" [--portname <portname>] [--baudrate <rate>] [--network] "
110 <<
"[--extrinsic <extrinsic.yaml>] [--constant-gain] [--help] [-h]" << std::endl
112 std::cout <<
"DESCRIPTION" << std::endl
113 <<
" --portname, -p <portname>" << std::endl
114 <<
" Set serial or tcp port name." << std::endl
116 <<
" --baudrate, -b <rate>" << std::endl
117 <<
" Set serial communication baud rate. Default: " << opt_baudrate <<
"." << std::endl
119 <<
" --network, -n" << std::endl
120 <<
" Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl
122 <<
" --extrinsic <extrinsic.yaml>" << std::endl
123 <<
" YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl
124 <<
" It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl
125 <<
" and camera frame." << std::endl
127 <<
" --constant-gain, -g" << std::endl
128 <<
" Constant gain value. Default value: " << opt_constant_gain << std::endl
130 <<
" --help, -h" << std::endl
131 <<
" Print this helper message. " << std::endl
133 std::cout <<
"EXAMPLE" << std::endl
134 <<
" - How to get network IP" << std::endl
136 <<
" $ " << argv[0] <<
" --portname COM1 --network" << std::endl
137 <<
" Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl
139 <<
" $ " << argv[0] <<
" --portname /dev/ttyUSB0 --network" << std::endl
140 <<
" Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
142 <<
" PTU HostName: PTU-5" << std::endl
143 <<
" PTU IP : 169.254.110.254" << std::endl
144 <<
" PTU Gateway : 0.0.0.0" << std::endl
145 <<
" - How to run this binary using network communication" << std::endl
146 <<
" $ " << argv[0] <<
" --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl;
155 std::cout <<
"Try to connect FLIR PTU to port: " << opt_portname <<
" with baudrate: " << opt_baudrate << std::endl;
156 robot.connect(opt_portname, opt_baudrate);
159 std::cout <<
"PTU HostName: " << robot.getNetworkHostName() << std::endl;
160 std::cout <<
"PTU IP : " << robot.getNetworkIP() << std::endl;
161 std::cout <<
"PTU Gateway : " << robot.getNetworkGateway() << std::endl;
173 eRc << 0, 0, 1, -1, 0, 0, 0, -1, 0;
174 etc << -0.1, -0.123, 0.035;
177 if (!opt_extrinsic.empty()) {
183 std::cout <<
"Considered extrinsic transformation eMc:\n" << eMc << std::endl;
187 std::cout <<
"Considered intrinsic camera parameters:\n" << cam <<
"\n";
189 #if defined(VISP_HAVE_X11)
191 #elif defined(VISP_HAVE_GDI)
197 detector.setDisplayTag(
true);
198 detector.setAprilTagQuadDecimate(2);
214 bool final_quit =
false;
215 bool send_velocities =
false;
225 std::vector<vpHomogeneousMatrix> cMo_vec;
228 while (!final_quit) {
233 detector.detect(I, opt_tag_size, cam, cMo_vec);
235 std::stringstream ss;
236 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
240 if (detector.getNbObjects() == 1) {
243 double Z = cMo_vec[0][2][3];
264 if (!send_velocities) {
277 send_velocities = !send_velocities;
290 std::cout <<
"Stop the robot " << std::endl;
294 std::cout <<
"Catch Flir Ptu exception: " << e.
getMessage() << std::endl;
303 #if !defined(VISP_HAVE_FLYCAPTURE)
304 std::cout <<
"Install FLIR Flycapture" << std::endl;
306 #if !defined(VISP_HAVE_FLIR_PTU_SDK)
307 std::cout <<
"Install FLIR PTU SDK." << std::endl;
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=nullptr)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
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 flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
const char * getMessage() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void set_xyZ(double x, double y, double Z)
void open(vpImage< unsigned char > &I)
void acquire(vpImage< unsigned char > &I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Implementation of a pose vector and operations on poses.
Error that can be emitted by the vpRobot class and its derivatives.
void get_eJe(vpMatrix &eJe) vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Implementation of a rotation matrix and operations on such kind of matrices.
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
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_)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector computeControlLaw()
Class that consider the case of a translation vector.
vpVelocityTwistMatrix get_cVe() const