Visual Servoing Platform  version 3.3.1 under development (2020-10-25)
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 
46 extern "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),
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 {
151  vpMatrix eJe;
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 {
178  vpMatrix fJe;
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  }
284  case vpRobot::MIXT_FRAME:
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
366  case vpRobot::TOOL_FRAME:
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
387  case vpRobot::JOINT_STATE: {
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) {
444  getJointPosition(q);
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 
531 void 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 
929 int vpRobotFlirPtu::rad2tics(int axis, double rad) { return (static_cast<int>(vpMath::deg(rad) / m_res[axis])); }
930 
939 double vpRobotFlirPtu::tics2deg(int axis, int tics) { return (tics * m_res[axis]); }
940 
949 double 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
954 void dummy_vpRobotFlirPtu(){};
955 #endif
void setTiltPosLimits(const vpColVector &tilt_limits)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix get_fMe()
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
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.
void setPanPosLimits(const vpColVector &pan_limits)
struct cerial * m_cer
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual ~vpRobotFlirPtu()
double maxTranslationVelocity
Definition: vpRobot.h:96
std::vector< double > m_res
Pan/tilt tic resolution in deg.
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
std::vector< int > m_vel_max_tics
Pan/tilt max velocity in robot tics unit.
vpColVector getTiltPosLimits()
double maxRotationVelocity
Definition: vpRobot.h:98
std::string getNetworkHostName()
void setPositioningVelocity(double velocity)
Initialize the position controller.
Definition: vpRobot.h:67
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix inverse() const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
vpColVector getPanPosLimits()
void getJointPosition(vpColVector &q)
std::string getNetworkGateway()
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
static void emergencyStop(int signo)
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpRobotStateType
Definition: vpRobot.h:64
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
std::vector< int > m_pos_max_tics
Pan min/max position in robot tics unit.
vpColVector getPanTiltVelMax()
static const double maxRotationVelocityDefault
Definition: vpRobot.h:99
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
double m_positioning_velocity
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
std::vector< int > m_pos_min_tics
Tilt min/max position in robot tics unit.
static double rad(double deg)
Definition: vpMath.h:110
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void connect(const std::string &portname, int baudrate=9600)
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static double deg(double rad)
Definition: vpMath.h:103
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
std::string getNetworkIP()
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpVelocityTwistMatrix get_cVe() const
vpMatrix get_eJe()
vpMatrix get_fJe()
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
void setJointVelocity(const vpColVector &qdot)