Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
vpRobotFlirPtu.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_FLIR_PTU_SDK
39 
40 #include <signal.h>
41 #include <stdexcept>
42 
43 extern "C" {
44 #include <cpi.h>
45 }
46 
52 #include <visp3/core/vpHomogeneousMatrix.h>
53 #include <visp3/robot/vpRobotFlirPtu.h>
54 
63 {
64  std::stringstream msg;
65  msg << "Stop the FLIR PTU by signal (" << signo << "): " << (char)7;
66  switch (signo) {
67  case SIGINT:
68  msg << "SIGINT (stop by ^C) ";
69  break;
70  case SIGSEGV:
71  msg << "SIGSEGV (stop due to a segmentation fault) ";
72  break;
73 #ifndef WIN32
74  case SIGBUS:
75  msg << "SIGBUS (stop due to a bus error) ";
76  break;
77  case SIGKILL:
78  msg << "SIGKILL (stop by CTRL \\) ";
79  break;
80  case SIGQUIT:
81  msg << "SIGQUIT ";
82  break;
83 #endif
84  default:
85  msg << signo << std::endl;
86  }
87 
89 }
90 
95 {
96  // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
97  // that is set to identity by default in the constructor.
98 
101 
102  // Set here the robot degrees of freedom number
103  nDof = 2; // Flir Ptu has 2 dof
104 }
105 
110  : m_eMc(), m_cer(nullptr), m_status(0), m_pos_max_tics(2), m_pos_min_tics(2), m_vel_max_tics(2), m_res(2),
111  m_connected(false), m_njoints(2), m_positioning_velocity(20.)
112 {
113  signal(SIGINT, vpRobotFlirPtu::emergencyStop);
114  signal(SIGSEGV, vpRobotFlirPtu::emergencyStop);
115 #ifndef WIN32
116  signal(SIGBUS, vpRobotFlirPtu::emergencyStop);
117  signal(SIGKILL, vpRobotFlirPtu::emergencyStop);
118  signal(SIGQUIT, vpRobotFlirPtu::emergencyStop);
119 #endif
120 
121  init();
122 }
123 
128 {
129  stopMotion();
130  disconnect();
131 }
132 
133 /*
134  At least one of these function has to be implemented to control the robot with a
135  Cartesian velocity:
136  - get_eJe()
137  - get_fJe()
138 */
139 
147 {
148  vpMatrix eJe;
149  vpColVector q;
151  eJe.resize(6, 2);
152  eJe = 0;
153  eJe[3][0] = -sin(q[1]);
154  eJe[4][1] = -1;
155  eJe[5][0] = -cos(q[1]);
156  return eJe;
157 }
158 
166 
174 {
175  vpMatrix fJe;
176  vpColVector q;
178  fJe.resize(6, 2);
179  fJe = 0;
180  fJe[3][1] = -sin(q[1]);
181  fJe[4][1] = -cos(q[1]);
182  fJe[5][0] = -1;
183 
184  return fJe;
185 }
186 
194 
200 {
202  vpColVector q;
204  double c1 = cos(q[0]);
205  double c2 = cos(q[1]);
206  double s1 = sin(q[0]);
207  double s2 = sin(q[1]);
208 
209  fMe[0][0] = c1 * c2;
210  fMe[0][1] = s1;
211  fMe[0][2] = -c1 * s2;
212 
213  fMe[1][0] = -s1 * c2;
214  fMe[1][1] = c1;
215  fMe[1][2] = s1 * s2;
216 
217  fMe[2][0] = s2;
218  fMe[2][1] = 0;
219  fMe[2][2] = c2;
220 
221  return fMe;
222 }
223 
237 {
239  cVe.build(m_eMc.inverse());
240 
241  return cVe;
242 }
243 
244 /*
245  At least one of these function has to be implemented to control the robot:
246  - setCartVelocity()
247  - setJointVelocity()
248 */
249 
257 {
258  if (v.size() != 6) {
260  "Cannot send a velocity-skew vector in tool frame that is not 6-dim (%d)", v.size()));
261  }
262 
263  vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
264  switch (frame) {
265  case vpRobot::TOOL_FRAME: {
266  // We have to transform the requested velocity in the end-effector frame.
267  // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
268  // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
269  // a velocity skew from tool (or camera) frame into end-effector frame
271  v_e = eVc * v;
272  break;
273  }
274 
277  v_e = v;
278  break;
279  }
281  case vpRobot::MIXT_FRAME:
282  // Out of the scope
283  break;
284  }
285 
286  // Implement your stuff here to send the end-effector velocity skew v_e
287  // - If the SDK allows to send cartesian velocities in the end-effector, it's done. Just wrap data in v_e
288  // - If the SDK allows to send cartesian velocities in the reference (or base) frame you have to implement
289  // the robot Jacobian in set_fJe() and call:
290  // vpColVector v = get_fJe().inverse() * v_e;
291  // At this point you have to wrap data in v that is the 6-dim velocity to apply to the robot
292  // - If the SDK allows to send only joint velocities you have to implement the robot Jacobian in set_eJe()
293  // and call:
294  // vpColVector qdot = get_eJe().inverse() * v_e;
295  // setJointVelocity(qdot);
296  // - If the SDK allows to send only a cartesian position trajectory of the end-effector position in the base frame
297  // called fMe (for fix frame to end-effector homogeneous transformation) you can transform the cartesian
298  // velocity in the end-effector into a displacement eMe using the exponetial map:
299  // double delta_t = 0.010; // in sec
300  // vpHomogenesousMatrix eMe = vpExponentialMap::direct(v_e, delta_t);
301  // vpHomogenesousMatrix fMe = getPosition(vpRobot::REFERENCE_FRAME);
302  // the new position to reach is than given by fMe * eMe
303  // vpColVector fpe(vpPoseVector(fMe * eMe));
304  // setPosition(vpRobot::REFERENCE_FRAME, fpe);
305 
306  std::cout << "Not implemented ! " << std::endl;
307  std::cout << "To implement me you need : " << std::endl;
308  std::cout << "\t to known the robot jacobian expressed in ";
309  std::cout << "the end-effector frame (eJe) " << std::endl;
310  std::cout << "\t the frame transformation between tool or camera frame ";
311  std::cout << "and end-effector frame (cMe)" << std::endl;
312 }
313 
319 {
320  if (!m_connected) {
321  disconnect();
322  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
323  }
324 
325  std::vector<int> vel_tics(2);
326 
327  for (int i = 0; i < 2; i++) {
328  vel_tics[i] = rad2tics(i, qdot[i]);
329  if (std::fabs(vel_tics[i]) > m_vel_max_tics[i]) {
330  disconnect();
331  throw(vpException(vpException::fatalError, "Cannot set joint %d velocity %f (deg/s). Out of limits [-%f, %f].", i,
332  vpMath::deg(qdot[i]), -tics2deg(i, m_vel_max_tics[i]), tics2deg(i, m_vel_max_tics[i])));
333  }
334  }
335 
336  if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_SPEED_SET, vel_tics[0]) ||
337  cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_SPEED_SET, vel_tics[1])) {
338  throw(vpException(vpException::fatalError, "Unable to set velocity."));
339  }
340 }
341 
350 {
353  "Cannot send a velocity to the robot. "
354  "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
355  "entering your control loop.");
356  }
357 
358  vpColVector vel_sat(6);
359 
360  // Velocity saturation
361  switch (frame) {
362  // Saturation in cartesian space
363  case vpRobot::TOOL_FRAME:
366  case vpRobot::MIXT_FRAME: {
367  if (vel.size() != 6) {
369  "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
370  }
371  vpColVector vel_max(6);
372 
373  for (unsigned int i = 0; i < 3; i++)
374  vel_max[i] = getMaxTranslationVelocity();
375  for (unsigned int i = 3; i < 6; i++)
376  vel_max[i] = getMaxRotationVelocity();
377 
378  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
379 
380  setCartVelocity(frame, vel_sat);
381  break;
382  }
383  // Saturation in joint space
384  case vpRobot::JOINT_STATE: {
385  if (vel.size() != static_cast<size_t>(nDof)) {
386  throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %d-dim vector (%d)",
387  nDof, vel.size()));
388  }
389  vpColVector vel_max(vel.size());
390 
391  // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
392  vel_max = getMaxRotationVelocity();
393 
394  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
395 
396  setJointVelocity(vel_sat);
397  }
398  }
399 }
400 
401 /*
402  THESE FUNCTIONS ARE NOT MANDATORY BUT ARE USUALLY USEFUL
403 */
404 
410 {
411  if (!m_connected) {
412  disconnect();
413  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
414  }
415 
416  std::vector<int> pos_tics(2);
417 
418  if (cpi_ptcmd(m_cer, &m_status, OP_PAN_CURRENT_POS_GET, &pos_tics[0])) {
419  disconnect();
420  throw(vpException(vpException::fatalError, "Unable to query pan position."));
421  }
422  if (cpi_ptcmd(m_cer, &m_status, OP_TILT_CURRENT_POS_GET, &pos_tics[1])) {
423  disconnect();
424  throw(vpException(vpException::fatalError, "Unable to query pan position."));
425  }
426 
427  q.resize(2);
428  for (int i = 0; i < 2; i++) {
429  q[i] = tics2rad(i, pos_tics[i]);
430  }
431 }
432 
439 {
440  if (frame == JOINT_STATE) {
441  getJointPosition(q);
442  }
443  else {
444  std::cout << "Not implemented ! " << std::endl;
445  }
446 }
447 
454 {
455  if (frame != vpRobot::JOINT_STATE) {
456  std::cout << "FLIR PTU positioning is not implemented in this frame" << std::endl;
457  return;
458  }
459 
460  if (q.size() != 2) {
461  disconnect();
462  throw(vpException(vpException::fatalError, "FLIR PTU has only %d joints. Cannot set a position that is %d-dim.",
463  m_njoints, q.size()));
464  }
465  if (!m_connected) {
466  disconnect();
467  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
468  }
469 
470  double vmin = 0.01, vmax = 100.;
471  if (m_positioning_velocity < vmin || m_positioning_velocity > vmax) {
472  disconnect();
473  throw(
474  vpException(vpException::fatalError, "FLIR PTU Positioning velocity %f is not in range [%f, %f]", vmin, vmax));
475  }
476 
477  std::vector<int> pos_tics(2);
478 
479  for (int i = 0; i < 2; i++) {
480  pos_tics[i] = rad2tics(i, q[i]);
481  if (pos_tics[i] < m_pos_min_tics[i] || pos_tics[i] > m_pos_max_tics[i]) {
482  disconnect();
483  throw(vpException(vpException::fatalError, "Cannot set joint %d position %f (deg). Out of limits [%f, %f].", i,
484  vpMath::deg(q[i]), tics2deg(i, m_pos_min_tics[i]), tics2deg(i, m_pos_max_tics[i])));
485  }
486  }
487 
488  // Set desired speed wrt max pan/tilt speed
489  if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_SPEED_SET, (int)(m_vel_max_tics[0] * m_positioning_velocity / 100.)) ||
490  cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_SPEED_SET,
491  (int)(m_vel_max_tics[1] * m_positioning_velocity / 100.))) {
492  disconnect();
493  throw(vpException(vpException::fatalError, "Setting FLIR pan/tilt positioning velocity failed"));
494  }
495 
496  if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_POS_SET, pos_tics[0]) ||
497  cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_POS_SET, pos_tics[1])) {
498  disconnect();
499  throw(vpException(vpException::fatalError, "FLIR PTU failed to go to position %d, %d (deg).", vpMath::deg(q[0]),
500  vpMath::deg(q[1])));
501  }
502 
503  if (cpi_block_until(m_cer, nullptr, nullptr, OP_PAN_CURRENT_POS_GET, pos_tics[0]) ||
504  cpi_block_until(m_cer, nullptr, nullptr, OP_TILT_CURRENT_POS_GET, pos_tics[1])) {
505  disconnect();
506  throw(vpException(vpException::fatalError, "FLIR PTU failed to wait until position %d, %d reached (deg)",
507  vpMath::deg(q[0]), vpMath::deg(q[1])));
508  }
509 }
510 
517 {
518  (void)frame;
519  (void)q;
520  std::cout << "Not implemented ! " << std::endl;
521 }
522 
529 void vpRobotFlirPtu::connect(const std::string &portname, int baudrate)
530 {
531  char errstr[128];
532 
533  if (m_connected) {
534  disconnect();
535  }
536 
537  if (portname.empty()) {
538  disconnect();
539  throw(vpException(vpException::fatalError, "Port name is required to connect to FLIR PTU."));
540  }
541 
542  if ((m_cer = (struct cerial *)malloc(sizeof(struct cerial))) == nullptr) {
543  disconnect();
544  throw(vpException(vpException::fatalError, "Out of memory during FLIR PTU connection."));
545  }
546 
547  // Open a port
548  if (ceropen(m_cer, portname.c_str(), 0)) {
549 #if WIN32
550  throw(vpException(vpException::fatalError, "Failed to open %s: %s.", portname.c_str(),
551  cerstrerror(m_cer, errstr, sizeof(errstr))));
552 #else
553  throw(vpException(vpException::fatalError, "Failed to open %s: %s.\nRun `sudo chmod a+rw %s`", portname.c_str(),
554  cerstrerror(m_cer, errstr, sizeof(errstr)), portname.c_str()));
555 #endif
556  }
557 
558  // Set baudrate
559  // ignore errors since not all devices are serial ports
560  cerioctl(m_cer, CERIAL_IOCTL_BAUDRATE_SET, &baudrate);
561 
562  // Flush any characters already buffered
563  cerioctl(m_cer, CERIAL_IOCTL_FLUSH_INPUT, nullptr);
564 
565  // Set two second timeout */
566  int timeout = 2000;
567  if (cerioctl(m_cer, CERIAL_IOCTL_TIMEOUT_SET, &timeout)) {
568  disconnect();
569  throw(vpException(vpException::fatalError, "cerial: timeout ioctl not supported."));
570  }
571 
572  // Sync and lock
573  int trial = 0;
574  do {
575  trial++;
576  } while (trial <= 3 && (cpi_resync(m_cer) || cpi_ptcmd(m_cer, &m_status, OP_NOOP)));
577  if (trial > 3) {
578  disconnect();
579  throw(vpException(vpException::fatalError, "Cannot communicate with FLIR PTU."));
580  }
581 
582  // Immediately execute commands (slave mode should be opt-in)
583  int rc;
584  if ((rc = cpi_ptcmd(m_cer, &m_status, OP_EXEC_MODE_SET, (cpi_enum)CPI_IMMEDIATE_MODE))) {
585  disconnect();
586  throw(vpException(vpException::fatalError, "Set Immediate Mode failed: %s", cpi_strerror(rc)));
587  }
588 
589  m_connected = true;
590 
591  getLimits();
592 }
593 
599 {
600  if (m_cer != nullptr) {
601  cerclose(m_cer);
602  free(m_cer);
603  m_cer = nullptr;
604  m_connected = false;
605  }
606 }
607 
612 {
613  if (!m_connected) {
614  disconnect();
615  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
616  }
617 
618  int status;
619 
620  if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_MAX_POSITION, &m_pos_max_tics[0])) ||
621  (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_MIN_POSITION, &m_pos_min_tics[0])) ||
622  (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_MAX_POSITION, &m_pos_max_tics[1])) ||
623  (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_MIN_POSITION, &m_pos_min_tics[1])) ||
624  (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_UPPER_SPEED_LIMIT_GET, &m_vel_max_tics[0])) ||
625  (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_UPPER_SPEED_LIMIT_GET, &m_vel_max_tics[1]))) {
626  disconnect();
627  throw(vpException(vpException::fatalError, "Failed to get limits (%d) %s.", status, cpi_strerror(status)));
628  }
629 
630  // Get the ptu resolution so we can convert the angles to ptu positions
631  if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_RESOLUTION, &m_res[0])) ||
632  (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_RESOLUTION, &m_res[1]))) {
633  disconnect();
634  throw(vpException(vpException::fatalError, "Failed to get resolution (%d) %s.", status, cpi_strerror(status)));
635  }
636 
637  for (size_t i = 0; i < 2; i++) {
638  m_res[i] /= 3600.; // Resolutions are in arc-seconds, but we want degrees
639  }
640 }
641 
649 {
650  if (!m_connected) {
651  disconnect();
652  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
653  }
654  vpColVector pan_pos_limits(2);
655  pan_pos_limits[0] = tics2rad(0, m_pos_min_tics[0]);
656  pan_pos_limits[1] = tics2rad(0, m_pos_max_tics[0]);
657 
658  return pan_pos_limits;
659 }
660 
668 {
669  if (!m_connected) {
670  disconnect();
671  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
672  }
673  vpColVector tilt_pos_limits(2);
674  tilt_pos_limits[0] = tics2rad(0, m_pos_min_tics[1]);
675  tilt_pos_limits[1] = tics2rad(0, m_pos_max_tics[1]);
676 
677  return tilt_pos_limits;
678 }
679 
687 {
688  if (!m_connected) {
689  disconnect();
690  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
691  }
692  vpColVector vel_max(2);
693  for (int i = 0; i < 2; i++) {
694  vel_max[i] = tics2rad(i, m_vel_max_tics[i]);
695  }
696  return vel_max;
697 }
698 
706 {
707  if (!m_connected) {
708  disconnect();
709  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
710  }
711  if (pan_limits.size() != 2) {
712  disconnect();
713  throw(vpException(vpException::fatalError, "Cannot set max position that is not a 2-dim vector."));
714  }
715  std::vector<int> pan_limits_tics(2);
716  for (int i = 0; i < 2; i++) {
717  pan_limits_tics[i] = rad2tics(i, pan_limits[i]);
718  }
719 
720  int status;
721  if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_USER_MIN_POS_SET, pan_limits_tics[0])) ||
722  (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_USER_MAX_POS_SET, pan_limits_tics[1]))) {
723  disconnect();
724  throw(vpException(vpException::fatalError, "Failed to set pan position limits (%d) %s.", status,
725  cpi_strerror(status)));
726  }
727 }
728 
736 {
737  if (!m_connected) {
738  disconnect();
739  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
740  }
741  if (tilt_limits.size() != 2) {
742  disconnect();
743  throw(vpException(vpException::fatalError, "Cannot set max position that is not a 2-dim vector."));
744  }
745  std::vector<int> tilt_limits_tics(2);
746  for (int i = 0; i < 2; i++) {
747  tilt_limits_tics[i] = rad2tics(i, tilt_limits[i]);
748  }
749 
750  int status;
751  if ((status = cpi_ptcmd(m_cer, &m_status, OP_TILT_USER_MIN_POS_SET, tilt_limits_tics[0])) ||
752  (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_USER_MAX_POS_SET, tilt_limits_tics[1]))) {
753  disconnect();
754  throw(vpException(vpException::fatalError, "Failed to set tilt position limits (%d) %s.", status,
755  cpi_strerror(status)));
756  }
757 }
758 
766 
775 {
776  if (!m_connected) {
777  return getRobotState();
778  }
779 
780  switch (newState) {
781  case vpRobot::STATE_STOP: {
782  // Start primitive STOP only if the current state is Velocity
784  stopMotion();
785 
786  // Set the PTU to pure velocity mode
787  if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
788  throw(vpException(vpException::fatalError, "Unable to set control mode independent."));
789  }
790  }
791  break;
792  }
795  std::cout << "Change the control mode from velocity to position control.\n";
796  stopMotion();
797 
798  // Set the PTU to pure velocity mode
799  if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
800  throw(vpException(vpException::fatalError, "Unable to set control mode independent."));
801  }
802 
803  }
804  else {
805  // std::cout << "Change the control mode from stop to position
806  // control.\n";
807  }
808  break;
809  }
812  std::cout << "Change the control mode from stop to velocity control.\n";
813 
814  // Set the PTU to pure velocity mode
815  if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_PURE_VELOCITY)) {
816  throw(vpException(vpException::fatalError, "Unable to set velocity control mode."));
817  }
818  }
819  break;
820  }
821  default:
822  break;
823  }
824 
825  return vpRobot::setRobotState(newState);
826 }
827 
832 {
833  if (!m_connected) {
834  return;
835  }
836 
837  if (cpi_ptcmd(m_cer, &m_status, OP_RESET, nullptr)) {
838  throw(vpException(vpException::fatalError, "Unable to reset PTU."));
839  }
840 }
841 
846 {
848  return;
849  }
850 
851  if (!m_connected) {
852  return;
853  }
854 
855  if (cpi_ptcmd(m_cer, &m_status, OP_HALT, nullptr)) {
856  throw(vpException(vpException::fatalError, "Unable to stop PTU."));
857  }
858 }
859 
866 {
867  if (!m_connected) {
868  disconnect();
869  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
870  }
871 
872  char str[64];
873  if (cpi_ptcmd(m_cer, &m_status, OP_NET_IP_GET, (int)sizeof(str), nullptr, &str)) {
874  throw(vpException(vpException::fatalError, "Unable to get Network IP."));
875  }
876 
877  return (std::string(str));
878 }
879 
886 {
887  if (!m_connected) {
888  disconnect();
889  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
890  }
891 
892  char str[64];
893  if (cpi_ptcmd(m_cer, &m_status, OP_NET_GATEWAY_GET, (int)sizeof(str), nullptr, &str)) {
894  throw(vpException(vpException::fatalError, "Unable to get Network Gateway."));
895  }
896 
897  return (std::string(str));
898 }
899 
906 {
907  if (!m_connected) {
908  disconnect();
909  throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
910  }
911 
912  char str[64];
913  if (cpi_ptcmd(m_cer, &m_status, OP_NET_HOSTNAME_GET, (int)sizeof(str), nullptr, &str)) {
914  throw(vpException(vpException::fatalError, "Unable to get Network hostname."));
915  }
916 
917  return (std::string(str));
918 }
919 
928 int vpRobotFlirPtu::rad2tics(int axis, double rad) { return (static_cast<int>(vpMath::deg(rad) / m_res[axis])); }
929 
938 double vpRobotFlirPtu::tics2deg(int axis, int tics) { return (tics * m_res[axis]); }
939 
948 double vpRobotFlirPtu::tics2rad(int axis, int tics) { return vpMath::rad(tics2deg(axis, tics)); }
949 
950 #elif !defined(VISP_BUILD_SHARED_LIBS)
951 // Work around to avoid warning: libvisp_robot.a(vpRobotFlirPtu.cpp.o) has
952 // no symbols
953 void dummy_vpRobotFlirPtu() { };
954 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:354
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:70
@ fatalError
Fatal error.
Definition: vpException.h:71
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ signalException
Signal exception returned after SIGINT (CTRL-C), SIGBUS, SIGSEGV, SIGSEGV (CTRL-),...
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
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 connect(const std::string &portname, int baudrate=9600)
vpColVector getTiltPosLimits()
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
vpColVector getPanPosLimits()
double m_positioning_velocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
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)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
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 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:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
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:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
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:198
double maxTranslationVelocity
Definition: vpRobot.h:96
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
double maxRotationVelocity
Definition: vpRobot.h:98
vpVelocityTwistMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)