ptu_manager.cpp

Go to the documentation of this file.

#include "CameraUtils/ptu_manager.h"

#ifndef Q_MOC_RUN
#include <sensor_msgs/JointState.h>
#include <ros/init.h>
#include <ptu_controller/PTUControllerClient.h>
#endif


PTU_Manager::PTU_Manager(double pan_min_angle, double pan_max_angle, double tilt_min_angle, double tilt_max_angle, int pan_step_count, int tilt_step_count)
{  
    ros::NodeHandle n_ = ros::NodeHandle();
    controller = new asr_flir_ptu_controller::PTUController(n_, "ptu_controller");
    //PTUMovementEndedCallback test( boost::bind( &PTU_Manager::ptu_callback, this, _1, _2) );
    //callbackPtr = PTUMovementEndedCallbackPtr(boost::make_shared<PTUMovementEndedCallback>(test));
    pan_step = 0;
    tilt_step = 0;
    pan_step_add = 1;
    this->pan_min_angle = pan_min_angle;
    this->pan_max_angle = pan_max_angle;
    this-> tilt_min_angle = tilt_min_angle;
    this->tilt_max_angle = tilt_max_angle;
    this->pan_stepsize = (pan_max_angle - pan_min_angle) / (double)pan_step_count;
    this->tilt_stepsize = (tilt_max_angle - tilt_min_angle) / (double)tilt_step_count;
}

void PTU_Manager::ptu_callback(double pan, double tilt)
{
    currentPan = pan;
    currentTilt = tilt;
    ptu_moved(pan, tilt);
}

bool PTU_Manager::setStartPose()
{
    pan_step = 0;
    tilt_step = 0;
    sendJoint(pan_min_angle, tilt_min_angle, true);
    return true ;
}

bool PTU_Manager::setNeutralPose()
{
    sendJoint(0,0,true);
    return true;
}

bool PTU_Manager::nextPose()
{
    pan_step += pan_step_add;
    if (pan_step*pan_stepsize + pan_min_angle > pan_max_angle || pan_step < 0)
    {
        pan_step_add *= -1;
        pan_step += pan_step_add;
        tilt_step ++;
    }
    if (tilt_step*tilt_stepsize + tilt_min_angle <= tilt_max_angle)
    {

        sendJoint(pan_min_angle + pan_step*pan_stepsize, tilt_min_angle + tilt_step*tilt_stepsize, false);
        return true;
    }
    else
    {
        return false;
    }
}

void PTU_Manager::sendJoint(double pan, double tilt, bool wait)
{
    ROS_DEBUG_STREAM("PTU: Sending joint to " << pan<< ", " << tilt);
    if (wait)
    {
        //controller->moveTo(pan, tilt);
    }
    else
    {
        //controller->asyncMoveTo(pan, tilt, callbackPtr);
    }
}

double PTU_Manager::getPan()
{
    return currentPan;
}

double PTU_Manager::getTilt()
{
   return currentTilt;
}