Visual Servoing Platform version 3.5.0
vpRobotFlirPtu.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for Flir Ptu Cpi robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_HAVE_FLIR_PTU_SDK
42
43#include <signal.h>
44#include <stdexcept>
45
46extern "C" {
47#include <cpi.h>
48}
49
55#include <visp3/core/vpHomogeneousMatrix.h>
56#include <visp3/robot/vpRobotFlirPtu.h>
57
66{
67 std::stringstream msg;
68 msg << "Stop the FLIR PTU by signal (" << signo << "): " << (char)7;
69 switch (signo) {
70 case SIGINT:
71 msg << "SIGINT (stop by ^C) ";
72 break;
73 case SIGSEGV:
74 msg << "SIGSEGV (stop due to a segmentation fault) ";
75 break;
76#ifndef WIN32
77 case SIGBUS:
78 msg << "SIGBUS (stop due to a bus error) ";
79 break;
80 case SIGKILL:
81 msg << "SIGKILL (stop by CTRL \\) ";
82 break;
83 case SIGQUIT:
84 msg << "SIGQUIT ";
85 break;
86#endif
87 default:
88 msg << signo << std::endl;
89 }
90
92}
93
98{
99 // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
100 // that is set to identity by default in the constructor.
101
104
105 // Set here the robot degrees of freedom number
106 nDof = 2; // Flir Ptu has 2 dof
107}
108
113 : m_eMc(), m_cer(NULL), m_status(0), m_pos_max_tics(2), m_pos_min_tics(2), m_vel_max_tics(2), m_res(2),
114 m_connected(false), m_njoints(2), m_positioning_velocity(20.)
115{
116 signal(SIGINT, vpRobotFlirPtu::emergencyStop);
117 signal(SIGSEGV, vpRobotFlirPtu::emergencyStop);
118#ifndef WIN32
119 signal(SIGBUS, vpRobotFlirPtu::emergencyStop);
120 signal(SIGKILL, vpRobotFlirPtu::emergencyStop);
121 signal(SIGQUIT, vpRobotFlirPtu::emergencyStop);
122#endif
123
124 init();
125}
126
131{
132 stopMotion();
133 disconnect();
134}
135
136/*
137 At least one of these function has to be implemented to control the robot with a
138 Cartesian velocity:
139 - get_eJe()
140 - get_fJe()
141*/
142
150{
152 vpColVector q;
154 eJe.resize(6, 2);
155 eJe = 0;
156 eJe[3][0] = -sin(q[1]);
157 eJe[4][1] = -1;
158 eJe[5][0] = -cos(q[1]);
159 return eJe;
160}
161
169
177{
179 vpColVector q;
181 fJe.resize(6, 2);
182 fJe = 0;
183 fJe[3][1] = -sin(q[1]);
184 fJe[4][1] = -cos(q[1]);
185 fJe[5][0] = -1;
186
187 return fJe;
188}
189
197
203{
205 vpColVector q;
207 double c1 = cos(q[0]);
208 double c2 = cos(q[1]);
209 double s1 = sin(q[0]);
210 double s2 = sin(q[1]);
211
212 fMe[0][0] = c1 * c2;
213 fMe[0][1] = s1;
214 fMe[0][2] = -c1 * s2;
215
216 fMe[1][0] = -s1 * c2;
217 fMe[1][1] = c1;
218 fMe[1][2] = s1 * s2;
219
220 fMe[2][0] = s2;
221 fMe[2][1] = 0;
222 fMe[2][2] = c2;
223
224 return fMe;
225}
226
240{
242 cVe.buildFrom(m_eMc.inverse());
243
244 return cVe;
245}
246
247/*
248 At least one of these function has to be implemented to control the robot:
249 - setCartVelocity()
250 - setJointVelocity()
251*/
252
260{
261 if (v.size() != 6) {
263 "Cannot send a velocity-skew vector in tool frame that is not 6-dim (%d)", v.size()));
264 }
265
266 vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
267 switch (frame) {
268 case vpRobot::TOOL_FRAME: {
269 // We have to transform the requested velocity in the end-effector frame.
270 // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
271 // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
272 // a velocity skew from tool (or camera) frame into end-effector frame
274 v_e = eVc * v;
275 break;
276 }
277
280 v_e = v;
281 break;
282 }
285 // Out of the scope
286 break;
287 }
288
289 // Implement your stuff here to send the end-effector velocity skew v_e
290 // - If the SDK allows to send cartesian velocities in the end-effector, it's done. Just wrap data in v_e
291 // - If the SDK allows to send cartesian velocities in the reference (or base) frame you have to implement
292 // the robot Jacobian in set_fJe() and call:
293 // vpColVector v = get_fJe().inverse() * v_e;
294 // At this point you have to wrap data in v that is the 6-dim velocity to apply to the robot
295 // - If the SDK allows to send only joint velocities you have to implement the robot Jacobian in set_eJe()
296 // and call:
297 // vpColVector qdot = get_eJe().inverse() * v_e;
298 // setJointVelocity(qdot);
299 // - If the SDK allows to send only a cartesian position trajectory of the end-effector position in the base frame
300 // called fMe (for fix frame to end-effector homogeneous transformation) you can transform the cartesian
301 // velocity in the end-effector into a displacement eMe using the exponetial map:
302 // double delta_t = 0.010; // in sec
303 // vpHomogenesousMatrix eMe = vpExponentialMap::direct(v_e, delta_t);
304 // vpHomogenesousMatrix fMe = getPosition(vpRobot::REFERENCE_FRAME);
305 // the new position to reach is than given by fMe * eMe
306 // vpColVector fpe(vpPoseVector(fMe * eMe));
307 // setPosition(vpRobot::REFERENCE_FRAME, fpe);
308
309 std::cout << "Not implemented ! " << std::endl;
310 std::cout << "To implement me you need : " << std::endl;
311 std::cout << "\t to known the robot jacobian expressed in ";
312 std::cout << "the end-effector frame (eJe) " << std::endl;
313 std::cout << "\t the frame transformation between tool or camera frame ";
314 std::cout << "and end-effector frame (cMe)" << std::endl;
315}
316
322{
323 if (!m_connected) {
324 disconnect();
325 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
326 }
327
328 std::vector<int> vel_tics(2);
329
330 for (int i = 0; i < 2; i++) {
331 vel_tics[i] = rad2tics(i, qdot[i]);
332 if (std::fabs(vel_tics[i]) > m_vel_max_tics[i]) {
333 disconnect();
334 throw(vpException(vpException::fatalError, "Cannot set joint %d velocity %f (deg/s). Out of limits [-%f, %f].", i,
335 vpMath::deg(qdot[i]), -tics2deg(i, m_vel_max_tics[i]), tics2deg(i, m_vel_max_tics[i])));
336 }
337 }
338
339 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_SPEED_SET, vel_tics[0]) ||
340 cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_SPEED_SET, vel_tics[1])) {
341 throw(vpException(vpException::fatalError, "Unable to set velocity."));
342 }
343}
344
353{
356 "Cannot send a velocity to the robot. "
357 "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
358 "entering your control loop.");
359 }
360
361 vpColVector vel_sat(6);
362
363 // Velocity saturation
364 switch (frame) {
365 // Saturation in cartesian space
369 case vpRobot::MIXT_FRAME: {
370 if (vel.size() != 6) {
372 "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
373 }
374 vpColVector vel_max(6);
375
376 for (unsigned int i = 0; i < 3; i++)
377 vel_max[i] = getMaxTranslationVelocity();
378 for (unsigned int i = 3; i < 6; i++)
379 vel_max[i] = getMaxRotationVelocity();
380
381 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
382
383 setCartVelocity(frame, vel_sat);
384 break;
385 }
386 // Saturation in joint space
388 if (vel.size() != static_cast<size_t>(nDof)) {
389 throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %d-dim vector (%d)",
390 nDof, vel.size()));
391 }
392 vpColVector vel_max(vel.size());
393
394 // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
395 vel_max = getMaxRotationVelocity();
396
397 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
398
399 setJointVelocity(vel_sat);
400 }
401 }
402}
403
404/*
405 THESE FUNCTIONS ARE NOT MENDATORY BUT ARE USUALLY USEFUL
406*/
407
413{
414 if (!m_connected) {
415 disconnect();
416 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
417 }
418
419 std::vector<int> pos_tics(2);
420
421 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_CURRENT_POS_GET, &pos_tics[0])) {
422 disconnect();
423 throw(vpException(vpException::fatalError, "Unable to query pan position."));
424 }
425 if (cpi_ptcmd(m_cer, &m_status, OP_TILT_CURRENT_POS_GET, &pos_tics[1])) {
426 disconnect();
427 throw(vpException(vpException::fatalError, "Unable to query pan position."));
428 }
429
430 q.resize(2);
431 for (int i = 0; i < 2; i++) {
432 q[i] = tics2rad(i, pos_tics[i]);
433 }
434}
435
442{
443 if (frame == JOINT_STATE) {
445 } else {
446 std::cout << "Not implemented ! " << std::endl;
447 }
448}
449
456{
457 if (frame != vpRobot::JOINT_STATE) {
458 std::cout << "FLIR PTU positioning is not implemented in this frame" << std::endl;
459 return;
460 }
461
462 if (q.size() != 2) {
463 disconnect();
464 throw(vpException(vpException::fatalError, "FLIR PTU has only %d joints. Cannot set a position that is %d-dim.",
465 m_njoints, q.size()));
466 }
467 if (!m_connected) {
468 disconnect();
469 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
470 }
471
472 double vmin = 0.01, vmax = 100.;
473 if (m_positioning_velocity < vmin || m_positioning_velocity > vmax) {
474 disconnect();
475 throw(
476 vpException(vpException::fatalError, "FLIR PTU Positioning velocity %f is not in range [%f, %f]", vmin, vmax));
477 }
478
479 std::vector<int> pos_tics(2);
480
481 for (int i = 0; i < 2; i++) {
482 pos_tics[i] = rad2tics(i, q[i]);
483 if (pos_tics[i] < m_pos_min_tics[i] || pos_tics[i] > m_pos_max_tics[i]) {
484 disconnect();
485 throw(vpException(vpException::fatalError, "Cannot set joint %d position %f (deg). Out of limits [%f, %f].", i,
486 vpMath::deg(q[i]), tics2deg(i, m_pos_min_tics[i]), tics2deg(i, m_pos_max_tics[i])));
487 }
488 }
489
490 // Set desired speed wrt max pan/tilt speed
491 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_SPEED_SET, (int)(m_vel_max_tics[0] * m_positioning_velocity / 100.)) ||
492 cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_SPEED_SET,
493 (int)(m_vel_max_tics[1] * m_positioning_velocity / 100.))) {
494 disconnect();
495 throw(vpException(vpException::fatalError, "Setting FLIR pan/tilt positioning velocity failed"));
496 }
497
498 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_POS_SET, pos_tics[0]) ||
499 cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_POS_SET, pos_tics[1])) {
500 disconnect();
501 throw(vpException(vpException::fatalError, "FLIR PTU failed to go to position %d, %d (deg).", vpMath::deg(q[0]),
502 vpMath::deg(q[1])));
503 }
504
505 if (cpi_block_until(m_cer, NULL, NULL, OP_PAN_CURRENT_POS_GET, pos_tics[0]) ||
506 cpi_block_until(m_cer, NULL, NULL, OP_TILT_CURRENT_POS_GET, pos_tics[1])) {
507 disconnect();
508 throw(vpException(vpException::fatalError, "FLIR PTU failed to wait until position %d, %d reached (deg)",
509 vpMath::deg(q[0]), vpMath::deg(q[1])));
510 }
511}
512
519{
520 (void)frame;
521 (void)q;
522 std::cout << "Not implemented ! " << std::endl;
523}
524
531void vpRobotFlirPtu::connect(const std::string &portname, int baudrate)
532{
533 char errstr[128];
534
535 if (m_connected) {
536 disconnect();
537 }
538
539 if (portname.empty()) {
540 disconnect();
541 throw(vpException(vpException::fatalError, "Port name is required to connect to FLIR PTU."));
542 }
543
544 if ((m_cer = (struct cerial *)malloc(sizeof(struct cerial))) == NULL) {
545 disconnect();
546 throw(vpException(vpException::fatalError, "Out of memory during FLIR PTU connection."));
547 }
548
549 // Open a port
550 if (ceropen(m_cer, portname.c_str(), 0)) {
551#if WIN32
552 throw(vpException(vpException::fatalError, "Failed to open %s: %s.", portname.c_str(),
553 cerstrerror(m_cer, errstr, sizeof(errstr))));
554#else
555 throw(vpException(vpException::fatalError, "Failed to open %s: %s.\nRun `sudo chmod a+rw %s`", portname.c_str(),
556 cerstrerror(m_cer, errstr, sizeof(errstr)), portname.c_str()));
557#endif
558 }
559
560 // Set baudrate
561 // ignore errors since not all devices are serial ports
562 cerioctl(m_cer, CERIAL_IOCTL_BAUDRATE_SET, &baudrate);
563
564 // Flush any characters already buffered
565 cerioctl(m_cer, CERIAL_IOCTL_FLUSH_INPUT, NULL);
566
567 // Set two second timeout */
568 int timeout = 2000;
569 if (cerioctl(m_cer, CERIAL_IOCTL_TIMEOUT_SET, &timeout)) {
570 disconnect();
571 throw(vpException(vpException::fatalError, "cerial: timeout ioctl not supported."));
572 }
573
574 // Sync and lock
575 int trial = 0;
576 do {
577 trial++;
578 } while (trial <= 3 && (cpi_resync(m_cer) || cpi_ptcmd(m_cer, &m_status, OP_NOOP)));
579 if (trial > 3) {
580 disconnect();
581 throw(vpException(vpException::fatalError, "Cannot communicate with FLIR PTU."));
582 }
583
584 // Immediately execute commands (slave mode should be opt-in)
585 int rc;
586 if ((rc = cpi_ptcmd(m_cer, &m_status, OP_EXEC_MODE_SET, (cpi_enum)CPI_IMMEDIATE_MODE))) {
587 disconnect();
588 throw(vpException(vpException::fatalError, "Set Immediate Mode failed: %s", cpi_strerror(rc)));
589 }
590
591 m_connected = true;
592
593 getLimits();
594}
595
601{
602 if (m_cer != NULL) {
603 cerclose(m_cer);
604 free(m_cer);
605 m_cer = NULL;
606 m_connected = false;
607 }
608}
609
614{
615 if (!m_connected) {
616 disconnect();
617 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
618 }
619
620 int status;
621
622 if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_MAX_POSITION, &m_pos_max_tics[0])) ||
623 (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_MIN_POSITION, &m_pos_min_tics[0])) ||
624 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_MAX_POSITION, &m_pos_max_tics[1])) ||
625 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_MIN_POSITION, &m_pos_min_tics[1])) ||
626 (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_UPPER_SPEED_LIMIT_GET, &m_vel_max_tics[0])) ||
627 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_UPPER_SPEED_LIMIT_GET, &m_vel_max_tics[1]))) {
628 disconnect();
629 throw(vpException(vpException::fatalError, "Failed to get limits (%d) %s.", status, cpi_strerror(status)));
630 }
631
632 // Get the ptu resolution so we can convert the angles to ptu positions
633 if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_RESOLUTION, &m_res[0])) ||
634 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_RESOLUTION, &m_res[1]))) {
635 disconnect();
636 throw(vpException(vpException::fatalError, "Failed to get resolution (%d) %s.", status, cpi_strerror(status)));
637 }
638
639 for (size_t i = 0; i < 2; i++) {
640 m_res[i] /= 3600.; // Resolutions are in arc-seconds, but we want degrees
641 }
642}
643
651{
652 if (!m_connected) {
653 disconnect();
654 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
655 }
656 vpColVector pan_pos_limits(2);
657 pan_pos_limits[0] = tics2rad(0, m_pos_min_tics[0]);
658 pan_pos_limits[1] = tics2rad(0, m_pos_max_tics[0]);
659
660 return pan_pos_limits;
661}
662
670{
671 if (!m_connected) {
672 disconnect();
673 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
674 }
675 vpColVector tilt_pos_limits(2);
676 tilt_pos_limits[0] = tics2rad(0, m_pos_min_tics[1]);
677 tilt_pos_limits[1] = tics2rad(0, m_pos_max_tics[1]);
678
679 return tilt_pos_limits;
680}
681
689{
690 if (!m_connected) {
691 disconnect();
692 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
693 }
694 vpColVector vel_max(2);
695 for (int i = 0; i < 2; i++) {
696 vel_max[i] = tics2rad(i, m_vel_max_tics[i]);
697 }
698 return vel_max;
699}
700
708{
709 if (!m_connected) {
710 disconnect();
711 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
712 }
713 if (pan_limits.size() != 2) {
714 disconnect();
715 throw(vpException(vpException::fatalError, "Cannot set max position that is not a 2-dim vector."));
716 }
717 std::vector<int> pan_limits_tics(2);
718 for (int i = 0; i < 2; i++) {
719 pan_limits_tics[i] = rad2tics(i, pan_limits[i]);
720 }
721
722 int status;
723 if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_USER_MIN_POS_SET, pan_limits_tics[0])) ||
724 (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_USER_MAX_POS_SET, pan_limits_tics[1]))) {
725 disconnect();
726 throw(vpException(vpException::fatalError, "Failed to set pan position limits (%d) %s.", status,
727 cpi_strerror(status)));
728 }
729}
730
738{
739 if (!m_connected) {
740 disconnect();
741 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
742 }
743 if (tilt_limits.size() != 2) {
744 disconnect();
745 throw(vpException(vpException::fatalError, "Cannot set max position that is not a 2-dim vector."));
746 }
747 std::vector<int> tilt_limits_tics(2);
748 for (int i = 0; i < 2; i++) {
749 tilt_limits_tics[i] = rad2tics(i, tilt_limits[i]);
750 }
751
752 int status;
753 if ((status = cpi_ptcmd(m_cer, &m_status, OP_TILT_USER_MIN_POS_SET, tilt_limits_tics[0])) ||
754 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_USER_MAX_POS_SET, tilt_limits_tics[1]))) {
755 disconnect();
756 throw(vpException(vpException::fatalError, "Failed to set tilt position limits (%d) %s.", status,
757 cpi_strerror(status)));
758 }
759}
760
768
777{
778 if (!m_connected) {
779 return getRobotState();
780 }
781
782 switch (newState) {
783 case vpRobot::STATE_STOP: {
784 // Start primitive STOP only if the current state is Velocity
786 stopMotion();
787
788 // Set the PTU to pure velocity mode
789 if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
790 throw(vpException(vpException::fatalError, "Unable to set control mode independent."));
791 }
792 }
793 break;
794 }
797 std::cout << "Change the control mode from velocity to position control.\n";
798 stopMotion();
799
800 // Set the PTU to pure velocity mode
801 if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
802 throw(vpException(vpException::fatalError, "Unable to set control mode independent."));
803 }
804
805 } else {
806 // std::cout << "Change the control mode from stop to position
807 // control.\n";
808 }
809 break;
810 }
813 std::cout << "Change the control mode from stop to velocity control.\n";
814
815 // Set the PTU to pure velocity mode
816 if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_PURE_VELOCITY)) {
817 throw(vpException(vpException::fatalError, "Unable to set velocity control mode."));
818 }
819 }
820 break;
821 }
822 default:
823 break;
824 }
825
826 return vpRobot::setRobotState(newState);
827}
828
833{
834 if (!m_connected) {
835 return;
836 }
837
838 if (cpi_ptcmd(m_cer, &m_status, OP_RESET, NULL)) {
839 throw(vpException(vpException::fatalError, "Unable to reset PTU."));
840 }
841}
842
847{
849 return;
850 }
851
852 if (!m_connected) {
853 return;
854 }
855
856 if (cpi_ptcmd(m_cer, &m_status, OP_HALT, NULL)) {
857 throw(vpException(vpException::fatalError, "Unable to stop PTU."));
858 }
859}
860
867{
868 if (!m_connected) {
869 disconnect();
870 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
871 }
872
873 char str[64];
874 if (cpi_ptcmd(m_cer, &m_status, OP_NET_IP_GET, (int)sizeof(str), NULL, &str)) {
875 throw(vpException(vpException::fatalError, "Unable to get Network IP."));
876 }
877
878 return (std::string(str));
879}
880
887{
888 if (!m_connected) {
889 disconnect();
890 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
891 }
892
893 char str[64];
894 if (cpi_ptcmd(m_cer, &m_status, OP_NET_GATEWAY_GET, (int)sizeof(str), NULL, &str)) {
895 throw(vpException(vpException::fatalError, "Unable to get Network Gateway."));
896 }
897
898 return (std::string(str));
899}
900
907{
908 if (!m_connected) {
909 disconnect();
910 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
911 }
912
913 char str[64];
914 if (cpi_ptcmd(m_cer, &m_status, OP_NET_HOSTNAME_GET, (int)sizeof(str), NULL, &str)) {
915 throw(vpException(vpException::fatalError, "Unable to get Network hostname."));
916 }
917
918 return (std::string(str));
919}
920
929int vpRobotFlirPtu::rad2tics(int axis, double rad) { return (static_cast<int>(vpMath::deg(rad) / m_res[axis])); }
930
939double vpRobotFlirPtu::tics2deg(int axis, int tics) { return (tics * m_res[axis]); }
940
949double vpRobotFlirPtu::tics2rad(int axis, int tics) { return vpMath::rad(tics2deg(axis, tics)); }
950
951#elif !defined(VISP_BUILD_SHARED_LIBS)
952// Work arround to avoid warning: libvisp_robot.a(vpRobotFlirPtu.cpp.o) has
953// no symbols
954void dummy_vpRobotFlirPtu(){};
955#endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Error that can be emited by the vpRobot class and its derivates.
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
std::vector< int > m_pos_max_tics
Pan min/max position in robot tics unit.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void connect(const std::string &portname, int baudrate=9600)
vpColVector getTiltPosLimits()
vpColVector getPanPosLimits()
double m_positioning_velocity
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
vpMatrix get_fJe()
std::string getNetworkGateway()
std::vector< double > m_res
Pan/tilt tic resolution in deg.
static void emergencyStop(int signo)
void setPositioningVelocity(double velocity)
virtual ~vpRobotFlirPtu()
std::string getNetworkIP()
std::vector< int > m_vel_max_tics
Pan/tilt max velocity in robot tics unit.
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
struct cerial * m_cer
vpVelocityTwistMatrix get_cVe() const
void getJointPosition(vpColVector &q)
std::string getNetworkHostName()
void setJointVelocity(const vpColVector &qdot)
void setPanPosLimits(const vpColVector &pan_limits)
vpMatrix get_eJe()
std::vector< int > m_pos_min_tics
Tilt min/max position in robot tics unit.
vpColVector getPanTiltVelMax()
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
vpMatrix get_fMe()
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void setTiltPosLimits(const vpColVector &tilt_limits)
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ TOOL_FRAME
Definition: vpRobot.h:84
@ MIXT_FRAME
Definition: vpRobot.h:86
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
static const double maxRotationVelocityDefault
Definition: vpRobot.h:99
vpRobotStateType
Definition: vpRobot.h:64
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double maxTranslationVelocity
Definition: vpRobot.h:96
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:98
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)