Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
testRobotFlirPtu.cpp

Test that show how to control FLIR PTU pan/tilt axis in position and velocity.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2019 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See http://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Test FLIR PTU interface.
*
* Authors:
* Fabien Spindler
*
*****************************************************************************/
#include <iostream>
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_FLIR_PTU_SDK
#include <visp3/robot/vpRobotFlirPtu.h>
int main(int argc, char **argv)
{
std::string opt_portname;
int opt_baudrate = 9600;
bool opt_network = false;
bool opt_reset = false;
if (argc == 1) {
std::cout << "To see how to use this test, run: " << argv[0] << " --help" << std::endl;
return EXIT_SUCCESS;
}
for (int i = 1; i < argc; i++) {
if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) {
opt_portname = std::string(argv[i + 1]);
}
else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) {
opt_baudrate = std::atoi(argv[i + 1]);
}
else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) {
opt_network = true;
}
else if ((std::string(argv[i]) == "--reset" || std::string(argv[i]) == "-r")) {
opt_reset = true;
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "SYNOPSIS" << std::endl
<< " " << argv[0] << " [--portname <portname>] [--baudrate <rate>] [--network] [--reset] [--help] [-h]" << std::endl << std::endl
<< "DESCRIPTION" << std::endl
<< " --portname, -p <portname>" << std::endl
<< " Set serial or tcp port name." << std::endl << std::endl
<< " --baudrate, -b <rate>" << std::endl
<< " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl << std::endl
<< " --network, -n" << std::endl
<< " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl << std::endl
<< " --reset, -r" << std::endl
<< " Reset PTU axis and exit. " << std::endl << std::endl
<< " --help, -h" << std::endl
<< " Print this helper message. " << std::endl << std::endl
<< "EXAMPLE" << std::endl
<< " - How to get network IP" << std::endl
#ifdef _WIN32
<< " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl
#else
<< " $ " << argv[0] << " --portname COM1 --network" << std::endl
#endif
<< " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
<< " PTU HostName: PTU-5" << std::endl
<< " PTU IP : 169.254.110.254" << std::endl
<< " PTU Gateway : 0.0.0.0" << std::endl
<< " - How to run this binary using serial communication" << std::endl
#ifdef _WIN32
<< " $ " << argv[0] << " --portname COM1" << std::endl
#else
<< " $ " << argv[0] << " --portname /dev/ttyUSB0" << std::endl
#endif
<< " - How to run this binary using network communication" << std::endl
<< " $ " << argv[0] << " --portname tcp:169.254.110.254" << std::endl;
return EXIT_SUCCESS;
}
}
if (opt_portname.empty()) {
std::cout << "Error, portname unspecified. Run " << argv[0] << " --help" << std::endl;
return EXIT_SUCCESS;
}
vpRobotFlirPtu robot;
try {
vpColVector q(2), q_mes;
int answer;
std::cout << "Try to connect FLIR PTU to port: " << opt_portname << " with baudrate: " << opt_baudrate << std::endl;
robot.connect(opt_portname, opt_baudrate);
if(opt_network) {
std::cout << "PTU HostName: " << robot.getNetworkHostName() <<std::endl;
std::cout << "PTU IP : " << robot.getNetworkIP() <<std::endl;
std::cout << "PTU Gateway : " << robot.getNetworkGateway() <<std::endl;
return EXIT_SUCCESS;
}
if(opt_reset) {
std::cout << "Reset PTU axis" <<std::endl;
robot.reset();
return EXIT_SUCCESS;
}
{
std::cout << "** Test limits getter" << std::endl;
std::cout << "Pan pos min/max [deg]: " << vpMath::deg(robot.getPanPosLimits()[0]) << " " << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl;
std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " " << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl;
std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " " << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl << std::endl;
}
{
std::cout << "** Test limits setter" << std::endl;
// Reduce pan/tilt position limits wrt factory settings
vpColVector pan_pos_limits(2), tilt_pos_limits(2);
pan_pos_limits[0] = vpMath::rad(-90);
pan_pos_limits[1] = vpMath::rad(90);
tilt_pos_limits[0] = vpMath::rad(-20);
tilt_pos_limits[1] = vpMath::rad(20);
robot.setPanPosLimits(pan_pos_limits);
robot.setTiltPosLimits(tilt_pos_limits);
std::cout << "Modified user min/max limits: " << std::endl;
std::cout << "Pan pos min/max [deg]: " << vpMath::deg(robot.getPanPosLimits()[0]) << " " << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl;
std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " " << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl;
std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " " << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl << std::endl;
}
{
std::cout << "** Test position getter" << std::endl;
robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
std::cout << "Current position [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl;
std::cout << "Initialisation done." << std::endl << std::endl;
}
{
std::cout << "** Test joint positioning" << std::endl;
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
robot.setMaxRotationVelocity(std::min(robot.getPanTiltVelMax()[0], robot.getPanTiltVelMax()[1]) / 2.); // 50% of the slowest axis
q = 0;
std::cout << "Set joint position [deg]: " << vpMath::deg(q[0]) << " " << vpMath::deg(q[1]) << std::endl;
std::cout << "Enter a caracter to apply" << std::endl;
scanf("%d", &answer);
robot.setPositioningVelocity(50);
robot.setPosition(vpRobot::JOINT_STATE, q);
robot.getPosition(vpRobot::JOINT_STATE, q_mes);
std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl << std::endl;
}
{
std::cout << "** Test joint positioning" << std::endl;
q[0] = vpMath::rad(10); // Pan position in rad
q[1] = vpMath::rad(20); // Tilt position in rad
std::cout << "Set joint position: " << vpMath::deg(q[0]) << " " << vpMath::deg(q[1]) << "[deg]" << std::endl;
std::cout << "Enter a caracter to apply" << std::endl;
scanf("%d", &answer);
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl << std::endl;
}
{
std::cout << "** Test joint velocity" << std::endl;
vpColVector qdot(2);
qdot[0] = vpMath::rad(-10); // Pan velocity in rad/s
qdot[1] = vpMath::rad(0); // Tilt velocity in rad/s
std::cout << "Set velocity for 4s: " << vpMath::deg(qdot[0]) << " " << vpMath::deg(qdot[1]) << " [deg/s]" << std::endl;
std::cout << "Enter a caracter to apply" << std::endl;
scanf("%d", &answer);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
double t_start = vpTime::measureTimeMs();
do {
robot.setVelocity(vpRobot::JOINT_STATE, qdot);
vpTime::sleepMs(40);
} while (vpTime::measureTimeMs() - t_start < 4000);
robot.setRobotState(vpRobot::STATE_STOP);
robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]" << std::endl << std::endl;
}
{
std::cout << "** Test cartesian velocity with robot Jacobien eJe" << std::endl;
vpColVector v_e(6, 0);
v_e[4] = vpMath::rad(5); //wy_e
v_e[5] = vpMath::rad(5); //wz_e
std::cout << "Set cartesian velocity in end-effector frame for 4s: " << v_e[0] << " " << v_e[1] << " " << v_e[2] << " [m/s] "
<< vpMath::deg(v_e[3]) << " " << vpMath::deg(v_e[4]) << " " << vpMath::deg(v_e[5]) << " [deg/s]" << std::endl;
std::cout << "Enter a caracter to apply" << std::endl;
scanf("%d", &answer);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
double t_start = vpTime::measureTimeMs();
do {
vpColVector qdot = robot.get_eJe().pseudoInverse() * v_e;
robot.setVelocity(vpRobot::JOINT_STATE, qdot);
vpTime::sleepMs(40);
} while (vpTime::measureTimeMs() - t_start < 4000);
robot.setRobotState(vpRobot::STATE_STOP);
robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]" << std::endl << std::endl;
}
std::cout << "** The end" << std::endl;
} catch (const vpRobotException &e) {
std::cout << "Catch Flir Ptu signal exception: " << e.getMessage() << std::endl;
robot.setRobotState(vpRobot::STATE_STOP);
} catch (const vpException &e) {
std::cout << "Catch Flir Ptu exception: " << e.getMessage() << std::endl;
robot.setRobotState(vpRobot::STATE_STOP);
}
return EXIT_SUCCESS;
}
#else
int main()
{
std::cout << "You do not have an Flir Ptu robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif