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)) && defined(VISP_HAVE_PUGIXML)
78 std::vector<vpImagePoint> &traj_ip)
83 traj_ip.push_back(ip);
87 traj_ip.push_back(ip);
89 for (
size_t j = 1; j < traj_ip.size(); j++) {
94 int main(
int argc,
char **argv)
96 std::string opt_portname;
97 int opt_baudrate = 9600;
98 bool opt_network =
false;
99 std::string opt_extrinsic;
100 std::string opt_intrinsic;
101 std::string opt_camera_name;
102 bool display_tag =
true;
103 int opt_quad_decimate = 2;
104 double opt_tag_size = 0.120;
105 bool opt_verbose =
false;
106 bool opt_plot =
false;
107 bool opt_adaptive_gain =
false;
108 bool opt_task_sequencing =
false;
109 double opt_constant_gain = 0.5;
110 bool opt_display_trajectory =
true;
111 double convergence_threshold = 0.00005;
114 std::cout <<
"To see how to use this example, run: " << argv[0] <<
" --help" << std::endl;
118 for (
int i = 1; i < argc; i++) {
119 if ((std::string(argv[i]) ==
"--portname" || std::string(argv[i]) ==
"-p") && (i + 1 < argc)) {
120 opt_portname = std::string(argv[i + 1]);
122 else if ((std::string(argv[i]) ==
"--baudrate" || std::string(argv[i]) ==
"-b") && (i + 1 < argc)) {
123 opt_baudrate = std::atoi(argv[i + 1]);
125 else if ((std::string(argv[i]) ==
"--network" || std::string(argv[i]) ==
"-n")) {
128 else if (std::string(argv[i]) ==
"--extrinsic" && i + 1 < argc) {
129 opt_extrinsic = std::string(argv[i + 1]);
131 else if (std::string(argv[i]) ==
"--intrinsic" && i + 1 < argc) {
132 opt_intrinsic = std::string(argv[i + 1]);
134 else if (std::string(argv[i]) ==
"--camera-name" && i + 1 < argc) {
135 opt_camera_name = std::string(argv[i + 1]);
137 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
140 else if (std::string(argv[i]) ==
"--plot" || std::string(argv[i]) ==
"-p") {
143 else if (std::string(argv[i]) ==
"--display-image-trajectory" || std::string(argv[i]) ==
"-traj") {
144 opt_display_trajectory =
true;
146 else if (std::string(argv[i]) ==
"--adaptive-gain" || std::string(argv[i]) ==
"-a") {
147 opt_adaptive_gain =
true;
149 else if (std::string(argv[i]) ==
"--constant-gain" || std::string(argv[i]) ==
"-g") {
150 opt_constant_gain = std::stod(argv[i + 1]);
152 else if (std::string(argv[i]) ==
"--task-sequencing") {
153 opt_task_sequencing =
true;
155 else if (std::string(argv[i]) ==
"--quad-decimate" && i + 1 < argc) {
156 opt_quad_decimate = std::stoi(argv[i + 1]);
158 if (std::string(argv[i]) ==
"--tag-size" && i + 1 < argc) {
159 opt_tag_size = std::stod(argv[i + 1]);
161 else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
162 convergence_threshold = 0.;
164 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
165 std::cout <<
"SYNOPSIS" << std::endl
166 <<
" " << argv[0] <<
" [--portname <portname>] [--baudrate <rate>] [--network] "
167 <<
"[--extrinsic <extrinsic.yaml>] [--intrinsic <intrinsic.xml>] [--camera-name <name>] "
168 <<
"[--quad-decimate <decimation>] [--tag-size <size>] "
169 <<
"[--adaptive-gain] [--constant-gain] [--display-image-trajectory] [--plot] [--task-sequencing] "
170 <<
"[--no-convergence-threshold] [--verbose] [--help] [-h]" << std::endl
172 std::cout <<
"DESCRIPTION" << std::endl
173 <<
" --portname, -p <portname>" << std::endl
174 <<
" Set serial or tcp port name." << std::endl
176 <<
" --baudrate, -b <rate>" << std::endl
177 <<
" Set serial communication baud rate. Default: " << opt_baudrate <<
"." << std::endl
179 <<
" --network, -n" << std::endl
180 <<
" Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl
182 <<
" --reset, -r" << std::endl
183 <<
" Reset PTU axis and exit. " << std::endl
185 <<
" --extrinsic <extrinsic.yaml>" << std::endl
186 <<
" YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl
187 <<
" It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl
188 <<
" and camera frame." << std::endl
190 <<
" --intrinsic <intrinsic.xml>" << std::endl
191 <<
" Intrinsic camera parameters obtained after camera calibration." << std::endl
193 <<
" --camera-name <name>" << std::endl
194 <<
" Name of the camera to consider in the xml file provided for intrinsic camera parameters."
197 <<
" --quad-decimate <decimation>" << std::endl
198 <<
" Decimation factor used to detect AprilTag. Default " << opt_quad_decimate <<
"." << std::endl
200 <<
" --tag-size <size>" << std::endl
201 <<
" Width in meter or the black part of the AprilTag used as target. Default " << opt_tag_size
204 <<
" --adaptive-gain, -a" << std::endl
205 <<
" Enable adaptive gain instead of constant gain to speed up convergence. " << std::endl
207 <<
" --constant-gain, -g" << std::endl
208 <<
" Constant gain value. Default value: " << opt_constant_gain << std::endl
210 <<
" --display-image-trajectory, -traj" << std::endl
211 <<
" Display the trajectory of the target cog in the image. " << std::endl
213 <<
" --plot, -p" << std::endl
214 <<
" Enable curve plotter. " << std::endl
216 <<
" --task-sequencing" << std::endl
217 <<
" Enable task sequencing that allows to smoothly control the velocity of the robot. " << std::endl
219 <<
" --no-convergence-threshold" << std::endl
220 <<
" Disable ending servoing when it reaches the desired position." << std::endl
222 <<
" --verbose, -v" << std::endl
223 <<
" Additional printings. " << std::endl
225 <<
" --help, -h" << std::endl
226 <<
" Print this helper message. " << std::endl
228 std::cout <<
"EXAMPLE" << std::endl
229 <<
" - How to get network IP" << std::endl
231 <<
" $ " << argv[0] <<
" --portname COM1 --network" << std::endl
232 <<
" Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl
234 <<
" $ " << argv[0] <<
" -p /dev/ttyUSB0 --network" << std::endl
235 <<
" Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
237 <<
" PTU HostName: PTU-5" << std::endl
238 <<
" PTU IP : 169.254.110.254" << std::endl
239 <<
" PTU Gateway : 0.0.0.0" << std::endl
240 <<
" - How to run this binary using network communication" << std::endl
241 <<
" $ " << argv[0] <<
" --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl;
250 std::cout <<
"Try to connect FLIR PTU to port: " << opt_portname <<
" with baudrate: " << opt_baudrate << std::endl;
251 robot.connect(opt_portname, opt_baudrate);
254 std::cout <<
"PTU HostName: " << robot.getNetworkHostName() << std::endl;
255 std::cout <<
"PTU IP : " << robot.getNetworkIP() << std::endl;
256 std::cout <<
"PTU Gateway : " << robot.getNetworkGateway() << std::endl;
268 eRc << 0, 0, 1, -1, 0, 0, 0, -1, 0;
269 etc << -0.1, -0.123, 0.035;
273 if (!opt_extrinsic.empty()) {
279 std::cout <<
"***************************************************************" << std::endl;
280 std::cout <<
"Warning, use hard coded values for extrinsic camera parameters." << std::endl;
281 std::cout <<
"Create a yaml file that contains the extrinsic:" << std::endl
283 <<
"$ cat eMc.yaml" << std::endl
284 <<
"rows: 4" << std::endl
285 <<
"cols: 4" << std::endl
286 <<
"data:" << std::endl
287 <<
" - [0, 0, 1, -0.1]" << std::endl
288 <<
" - [-1, 0, 0, -0.123]" << std::endl
289 <<
" - [0, -1, 0, 0.035]" << std::endl
290 <<
" - [0, 0, 0, 1]" << std::endl
292 <<
"and load this file with [--extrinsic <extrinsic.yaml] command line option, like:" << std::endl
294 <<
"$ " << argv[0] <<
"-p " << opt_portname <<
" --extrinsic eMc.yaml" << std::endl
296 std::cout <<
"***************************************************************" << std::endl;
299 std::cout <<
"Considered extrinsic transformation eMc:\n" << eMc << std::endl;
305 if (!opt_intrinsic.empty() && !opt_camera_name.empty()) {
310 std::cout <<
"***************************************************************" << std::endl;
311 std::cout <<
"Warning, use hard coded values for intrinsic camera parameters." << std::endl;
312 std::cout <<
"Calibrate your camera and load the parameters from command line options, like:" << std::endl
314 <<
"$ " << argv[0] <<
"-p " << opt_portname <<
" --intrinsic camera.xml --camera-name \"Camera\""
317 std::cout <<
"***************************************************************" << std::endl;
320 std::cout <<
"Considered intrinsic camera parameters:\n" << cam <<
"\n";
322 #if defined(VISP_HAVE_X11)
324 #elif defined(VISP_HAVE_GDI)
331 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
332 detector.setDisplayTag(display_tag);
333 detector.setAprilTagQuadDecimate(opt_quad_decimate);
348 if (opt_adaptive_gain) {
356 vpPlot *plotter =
nullptr;
360 plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
361 "Real time curves plotter");
362 plotter->
setTitle(0,
"Visual features error");
363 plotter->
setTitle(1,
"Joint velocities");
366 plotter->
setLegend(0, 0,
"error_feat_p_x");
367 plotter->
setLegend(0, 1,
"error_feat_p_y");
372 bool final_quit =
false;
373 bool has_converged =
false;
374 bool send_velocities =
false;
375 bool servo_started =
false;
376 std::vector<vpImagePoint> traj_cog;
384 std::cout << cVe << std::endl;
389 while (!has_converged && !final_quit) {
396 std::vector<vpHomogeneousMatrix> cMo_vec;
397 detector.detect(I, opt_tag_size, cam, cMo_vec);
399 std::stringstream ss;
400 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
406 if (detector.getNbObjects() == 1) {
409 double Z = cMo_vec[0][2][3];
415 std::cout <<
"Z: " << Z << std::endl;
424 if (opt_task_sequencing) {
425 if (!servo_started) {
426 if (send_velocities) {
427 servo_started =
true;
441 if (opt_display_trajectory) {
442 display_point_trajectory(I, cog, traj_cog);
447 plotter->
plot(1, iter_plot, qdot);
452 std::cout <<
"qdot: " << qdot.t() << std::endl;
457 ss <<
"error: " << error;
461 std::cout <<
"error: " << error << std::endl;
463 if (error < convergence_threshold) {
464 has_converged =
true;
465 std::cout <<
"Servo task has converged"
474 if (!send_velocities) {
490 send_velocities = !send_velocities;
503 std::cout <<
"Stop the robot " << std::endl;
506 if (opt_plot && plotter !=
nullptr) {
512 while (!final_quit) {
528 std::cout <<
"Catch Flir Ptu signal exception: " << e.
getMessage() << std::endl;
532 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
533 std::cout <<
"Stop the robot " << std::endl;
542 #if !defined(VISP_HAVE_FLYCAPTURE)
543 std::cout <<
"Install FLIR Flycapture" << std::endl;
545 #if !defined(VISP_HAVE_FLIR_PTU_SDK)
546 std::cout <<
"Install FLIR PTU SDK." << std::endl;
548 #if !defined(VISP_HAVE_PUGIXML)
549 std::cout <<
"pugixml built-in 3rdparty is requested." << std::endl;
Adaptive gain computation.
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=nullptr)
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor green
@ 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 displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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)
error that can be emitted by ViSP classes.
const char * getMessage() const
const char * what() 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 ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
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)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setTitle(unsigned int graphNum, const std::string &title)
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 getError() const
vpColVector computeControlLaw()
Class that consider the case of a translation vector.
vpVelocityTwistMatrix get_cVe() const
XML parser to load and save intrinsic camera parameters.
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, bool verbose=true)
VISP_EXPORT double measureTimeMs()