36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_FLIR_PTU_SDK
52 #include <visp3/core/vpHomogeneousMatrix.h>
53 #include <visp3/robot/vpRobotFlirPtu.h>
64 std::stringstream msg;
65 msg <<
"Stop the FLIR PTU by signal (" << signo <<
"): " << (char)7;
68 msg <<
"SIGINT (stop by ^C) ";
71 msg <<
"SIGSEGV (stop due to a segmentation fault) ";
75 msg <<
"SIGBUS (stop due to a bus error) ";
78 msg <<
"SIGKILL (stop by CTRL \\) ";
85 msg << signo << std::endl;
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.)
153 eJe[3][0] = -sin(q[1]);
155 eJe[5][0] = -cos(q[1]);
180 fJe[3][1] = -sin(q[1]);
181 fJe[4][1] = -cos(q[1]);
204 double c1 = cos(q[0]);
205 double c2 = cos(q[1]);
206 double s1 = sin(q[0]);
207 double s2 = sin(q[1]);
211 fMe[0][2] = -c1 * s2;
213 fMe[1][0] = -s1 * c2;
260 "Cannot send a velocity-skew vector in tool frame that is not 6-dim (%d)", v.
size()));
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;
325 std::vector<int> vel_tics(2);
327 for (
int i = 0; i < 2; i++) {
328 vel_tics[i] = rad2tics(i, qdot[i]);
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])) {
353 "Cannot send a velocity to the robot. "
354 "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
355 "entering your control loop.");
367 if (vel.
size() != 6) {
369 "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.
size()));
373 for (
unsigned int i = 0; i < 3; i++)
375 for (
unsigned int i = 3; i < 6; i++)
385 if (vel.
size() !=
static_cast<size_t>(
nDof)) {
416 std::vector<int> pos_tics(2);
418 if (cpi_ptcmd(
m_cer, &
m_status, OP_PAN_CURRENT_POS_GET, &pos_tics[0])) {
422 if (cpi_ptcmd(
m_cer, &
m_status, OP_TILT_CURRENT_POS_GET, &pos_tics[1])) {
428 for (
int i = 0; i < 2; i++) {
429 q[i] = tics2rad(i, pos_tics[i]);
443 std::cout <<
"Not implemented ! " << std::endl;
455 std::cout <<
"FLIR PTU positioning is not implemented in this frame" << std::endl;
469 double vmin = 0.01, vmax = 100.;
470 if (m_positioning_velocity < vmin || m_positioning_velocity > vmax) {
476 std::vector<int> pos_tics(2);
478 for (
int i = 0; i < 2; i++) {
479 pos_tics[i] = rad2tics(i, q[i]);
495 if (cpi_ptcmd(
m_cer, &
m_status, OP_PAN_DESIRED_POS_SET, pos_tics[0]) ||
496 cpi_ptcmd(
m_cer, &
m_status, OP_TILT_DESIRED_POS_SET, pos_tics[1])) {
502 if (cpi_block_until(
m_cer,
nullptr,
nullptr, OP_PAN_CURRENT_POS_GET, pos_tics[0]) ||
503 cpi_block_until(
m_cer,
nullptr,
nullptr, OP_TILT_CURRENT_POS_GET, pos_tics[1])) {
519 std::cout <<
"Not implemented ! " << std::endl;
536 if (portname.empty()) {
541 if ((
m_cer = (
struct cerial *)malloc(
sizeof(
struct cerial))) ==
nullptr) {
547 if (ceropen(
m_cer, portname.c_str(), 0)) {
550 cerstrerror(
m_cer, errstr,
sizeof(errstr))));
553 cerstrerror(
m_cer, errstr,
sizeof(errstr)), portname.c_str()));
559 cerioctl(
m_cer, CERIAL_IOCTL_BAUDRATE_SET, &baudrate);
562 cerioctl(
m_cer, CERIAL_IOCTL_FLUSH_INPUT,
nullptr);
566 if (cerioctl(
m_cer, CERIAL_IOCTL_TIMEOUT_SET, &timeout)) {
583 if ((rc = cpi_ptcmd(
m_cer, &
m_status, OP_EXEC_MODE_SET, (cpi_enum)CPI_IMMEDIATE_MODE))) {
599 if (
m_cer !=
nullptr) {
636 for (
size_t i = 0; i < 2; i++) {
657 return pan_pos_limits;
676 return tilt_pos_limits;
692 for (
int i = 0; i < 2; i++) {
710 if (pan_limits.
size() != 2) {
714 std::vector<int> pan_limits_tics(2);
715 for (
int i = 0; i < 2; i++) {
716 pan_limits_tics[i] = rad2tics(i, pan_limits[i]);
720 if ((status = cpi_ptcmd(
m_cer, &
m_status, OP_PAN_USER_MIN_POS_SET, pan_limits_tics[0])) ||
721 (status = cpi_ptcmd(
m_cer, &
m_status, OP_PAN_USER_MAX_POS_SET, pan_limits_tics[1]))) {
724 cpi_strerror(status)));
740 if (tilt_limits.
size() != 2) {
744 std::vector<int> tilt_limits_tics(2);
745 for (
int i = 0; i < 2; i++) {
746 tilt_limits_tics[i] = rad2tics(i, tilt_limits[i]);
750 if ((status = cpi_ptcmd(
m_cer, &
m_status, OP_TILT_USER_MIN_POS_SET, tilt_limits_tics[0])) ||
751 (status = cpi_ptcmd(
m_cer, &
m_status, OP_TILT_USER_MAX_POS_SET, tilt_limits_tics[1]))) {
754 cpi_strerror(status)));
786 if (cpi_ptcmd(
m_cer, &
m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
794 std::cout <<
"Change the control mode from velocity to position control.\n";
798 if (cpi_ptcmd(
m_cer, &
m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
810 std::cout <<
"Change the control mode from stop to velocity control.\n";
813 if (cpi_ptcmd(
m_cer, &
m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_PURE_VELOCITY)) {
871 if (cpi_ptcmd(
m_cer, &
m_status, OP_NET_IP_GET, (
int)
sizeof(str),
nullptr, &str)) {
875 return (std::string(str));
891 if (cpi_ptcmd(
m_cer, &
m_status, OP_NET_GATEWAY_GET, (
int)
sizeof(str),
nullptr, &str)) {
895 return (std::string(str));
911 if (cpi_ptcmd(
m_cer, &
m_status, OP_NET_HOSTNAME_GET, (
int)
sizeof(str),
nullptr, &str)) {
915 return (std::string(str));
926 int vpRobotFlirPtu::rad2tics(
int axis,
double rad) {
return (
static_cast<int>(
vpMath::deg(rad) /
m_res[axis])); }
936 double vpRobotFlirPtu::tics2deg(
int axis,
int tics) {
return (tics *
m_res[axis]); }
946 double vpRobotFlirPtu::tics2rad(
int axis,
int tics) {
return vpMath::rad(tics2deg(axis, tics)); }
948 #elif !defined(VISP_BUILD_SHARED_LIBS)
951 void dummy_vpRobotFlirPtu(){};
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
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-),...
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()
vpColVector getPanPosLimits()
double m_positioning_velocity
std::string getNetworkGateway()
std::vector< double > m_res
Pan/tilt tic resolution in deg.
static void emergencyStop(int signo)
void setPositioningVelocity(double velocity)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) 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)
vpVelocityTwistMatrix get_cVe() const
void getJointPosition(vpColVector &q)
std::string getNetworkHostName()
void setJointVelocity(const vpColVector &qdot)
void setPanPosLimits(const vpColVector &pan_limits)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q) override
std::vector< int > m_pos_min_tics
Tilt min/max position in robot tics unit.
vpColVector getPanTiltVelMax()
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) override
void setTiltPosLimits(const vpColVector &tilt_limits)
int nDof
number of degrees of freedom
static const double maxTranslationVelocityDefault
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
virtual vpRobotStateType getRobotState(void) const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
static const double maxRotationVelocityDefault
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
double getMaxRotationVelocity(void) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double maxTranslationVelocity
double getMaxTranslationVelocity(void) const
double maxRotationVelocity
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)