marker_manager.cpp

Go to the documentation of this file.

#include "CameraUtils/marker_manager.h"
#include <QMetaType>

Marker_Manager::Marker_Manager(double timeout, double earlyabort_timeout, QObject *parent) :
    QThread(parent)
{
    qRegisterMetaType<Eigen::Matrix4d>("Eigen::Matrix4d");
    qRegisterMetaType<std::string>("std::string");
    nh = ros::NodeHandle(ros::this_node::getName());

    client_GetRecognizer = nh.serviceClient<asr_aruco_marker_recognition::GetRecognizer>("/asr_aruco_marker_recognition/get_recognizer");
    client_ReleaseRecognizer = nh.serviceClient<asr_aruco_marker_recognition::ReleaseRecognizer>("/asr_aruco_marker_recognition/release_recognizer");

    this->timeout = timeout;
    this->earlyabort_timeout = earlyabort_timeout;
    ROS_INFO_STREAM("Marker Recognition: Timeout = " << timeout);
    ROS_INFO_STREAM("Marker Recognition: Early Abort = " << earlyabort_timeout);
    if (timeout-earlyabort_timeout <= 0.0)
    {
        ROS_ERROR("Marker Recognition: Early Abort was invalid");
        earlyabort_timeout = 0.0;
    }
}

void Marker_Manager::run()
{
    if (getRecognizer())
    {
        //Start recognition
        recognizedMarker = false;

        ros::Duration d = ros::Duration(earlyabort_timeout, 0);
        d.sleep();

        //Early abort
        if (!recognizedMarker)
        {
            ROS_INFO("No Marker found -> Early abort.");
            markerFound("", Eigen::Matrix4d::Identity());
            releaseRecognizer();

        }
        else
        {
            d = ros::Duration(timeout-earlyabort_timeout, 0);
            d.sleep();

            markerFound("", Eigen::Matrix4d::Identity());
            releaseRecognizer();
        }
    }
    else
    {
        ROS_ERROR("Could not start recognition.");
    }
}

void Marker_Manager::markerRecognitionCallback(const asr_msgs::AsrObject::ConstPtr object)
{
    asr_msgs::AsrObject o = *object;
    recognizedMarker = true;
    std::string ID = o.type;
    Eigen::Affine3d transform;
    if(o.sampledPoses.size() > 0)
    {
        tf::poseMsgToEigen(o.sampledPoses.at(0).pose, transform);
        markerFound(ID, transform.matrix());
    }
}

bool Marker_Manager::releaseRecognizer()
{
    a.shutdown();
    asr_aruco_marker_recognition::ReleaseRecognizer release;
    return client_ReleaseRecognizer.call(release);
}

bool Marker_Manager::getRecognizer()
{
    a = nh.subscribe("/stereo/objects", 1000, &Marker_Manager::markerRecognitionCallback, this);
    asr_aruco_marker_recognition::GetRecognizer get;
    return client_GetRecognizer.call(get);
}