60#include <visp3/core/vpCameraParameters.h>
61#include <visp3/core/vpXmlParserCamera.h>
62#include <visp3/detection/vpDetectorAprilTag.h>
63#include <visp3/gui/vpDisplayGDI.h>
64#include <visp3/gui/vpDisplayX.h>
65#include <visp3/gui/vpPlot.h>
66#include <visp3/io/vpImageIo.h>
67#include <visp3/robot/vpRobotFlirPtu.h>
68#include <visp3/sensor/vpFlyCaptureGrabber.h>
69#include <visp3/visual_features/vpFeatureBuilder.h>
70#include <visp3/visual_features/vpFeaturePoint.h>
71#include <visp3/vs/vpServo.h>
72#include <visp3/vs/vpServoDisplay.h>
74#if defined(VISP_HAVE_FLIR_PTU_SDK) && defined(VISP_HAVE_FLYCAPTURE) && \
75 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
77int main(
int argc,
char **argv)
79 std::string opt_portname;
80 int opt_baudrate = 9600;
81 bool opt_network =
false;
82 std::string opt_extrinsic;
83 double opt_tag_size = 0.120;
84 double opt_constant_gain = 0.5;
87 std::cout <<
"To see how to use this example, run: " << argv[0] <<
" --help" << std::endl;
91 for (
int i = 1; i < argc; i++) {
92 if ((std::string(argv[i]) ==
"--portname" || std::string(argv[i]) ==
"-p") && (i + 1 < argc)) {
93 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]);
96 }
else if ((std::string(argv[i]) ==
"--network" || std::string(argv[i]) ==
"-n")) {
98 }
else if (std::string(argv[i]) ==
"--extrinsic" && i + 1 < argc) {
99 opt_extrinsic = std::string(argv[i + 1]);
100 }
else if (std::string(argv[i]) ==
"--constant-gain" || std::string(argv[i]) ==
"-g") {
101 opt_constant_gain = std::stod(argv[i + 1]);
103 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
104 std::cout <<
"SYNOPSIS" << std::endl
105 <<
" " << argv[0] <<
" [--portname <portname>] [--baudrate <rate>] [--network] "
106 <<
"[--extrinsic <extrinsic.yaml>] [--constant-gain] [--help] [-h]" << std::endl
108 std::cout <<
"DESCRIPTION" << std::endl
109 <<
" --portname, -p <portname>" << std::endl
110 <<
" Set serial or tcp port name." << std::endl
112 <<
" --baudrate, -b <rate>" << std::endl
113 <<
" Set serial communication baud rate. Default: " << opt_baudrate <<
"." << std::endl
115 <<
" --network, -n" << std::endl
116 <<
" Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl
118 <<
" --extrinsic <extrinsic.yaml>" << std::endl
119 <<
" YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl
120 <<
" It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl
121 <<
" and camera frame." << std::endl
123 <<
" --constant-gain, -g" << std::endl
124 <<
" Constant gain value. Default value: " << opt_constant_gain << std::endl
126 <<
" --help, -h" << std::endl
127 <<
" Print this helper message. " << std::endl
129 std::cout <<
"EXAMPLE" << std::endl
130 <<
" - How to get network IP" << std::endl
132 <<
" $ " << argv[0] <<
" --portname COM1 --network" << std::endl
133 <<
" Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl
135 <<
" $ " << argv[0] <<
" --portname /dev/ttyUSB0 --network" << std::endl
136 <<
" Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
138 <<
" PTU HostName: PTU-5" << std::endl
139 <<
" PTU IP : 169.254.110.254" << std::endl
140 <<
" PTU Gateway : 0.0.0.0" << std::endl
141 <<
" - How to run this binary using network communication" << std::endl
142 <<
" $ " << argv[0] <<
" --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl;
151 std::cout <<
"Try to connect FLIR PTU to port: " << opt_portname <<
" with baudrate: " << opt_baudrate << std::endl;
152 robot.connect(opt_portname, opt_baudrate);
155 std::cout <<
"PTU HostName: " << robot.getNetworkHostName() << std::endl;
156 std::cout <<
"PTU IP : " << robot.getNetworkIP() << std::endl;
157 std::cout <<
"PTU Gateway : " << robot.getNetworkGateway() << std::endl;
169 eRc << 0, 0, 1, -1, 0, 0, 0, -1, 0;
170 etc << -0.1, -0.123, 0.035;
173 if (!opt_extrinsic.empty()) {
179 std::cout <<
"Considered extrinsic transformation eMc:\n" << eMc << std::endl;
183 std::cout <<
"Considered intrinsic camera parameters:\n" << cam <<
"\n";
185#if defined(VISP_HAVE_X11)
187#elif defined(VISP_HAVE_GDI)
193 detector.setDisplayTag(
true);
194 detector.setAprilTagQuadDecimate(2);
210 bool final_quit =
false;
211 bool send_velocities =
false;
221 std::vector<vpHomogeneousMatrix> cMo_vec;
224 while (!final_quit) {
229 detector.detect(I, opt_tag_size, cam, cMo_vec);
231 std::stringstream ss;
232 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
236 if (detector.getNbObjects() == 1) {
239 double Z = cMo_vec[0][2][3];
260 if (!send_velocities) {
273 send_velocities = !send_velocities;
286 std::cout <<
"Stop the robot " << std::endl;
289 std::cout <<
"Catch Flir Ptu exception: " << e.
getMessage() << std::endl;
298#if !defined(VISP_HAVE_FLYCAPTURE)
299 std::cout <<
"Install FLIR Flycapture" << std::endl;
301#if !defined(VISP_HAVE_FLIR_PTU_SDK)
302 std::cout <<
"Install FLIR PTU SDK." << std::endl;
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
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 ...
Definition of the vpImage class member functions.
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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ 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 set_cVe(const vpVelocityTwistMatrix &cVe_)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix get_cVe() const