recognizer_prediction_psm_client.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "asr_recognizer_prediction_psm/recognizer_prediction_psm.h"
#include <cstdlib>
#include <asr_msgs/AsrObject.h>

#include <ros/package.h>


int main(int argc, char **argv)
{
    ros::init(argc, argv, "recognizer_prediction_psm_client");
    if (argc != 2)
    {
        ROS_INFO("usage: recognizer_prediction_psm_client integer");
        return 1;
    }

    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<asr_recognizer_prediction_psm::recognizer_prediction_psm>("recognizer_prediction_psm");
    asr_recognizer_prediction_psm::recognizer_prediction_psm srv;


    std::string path = ros::package::getPath("asr_recognizer_prediction_psm") + "/models/dome_scene1.xml";
    std::vector<std::string> scenes;
    scenes.push_back("background");
    scenes.push_back("marker_scene1");
    scenes.push_back("marker_scene2");

    std::vector<float> probs;
    probs.push_back(0.25f);
    probs.push_back(0.5f);
    probs.push_back(0.25);
    std::vector<asr_msgs::AsrObject> objects;

    // Test object
    {
        geometry_msgs::Point point;
        point.x = 0.31f;
        point.y = 0.35f;
        point.z = 1.37f;

        geometry_msgs::Quaternion q;
        q.w = 1.0f;
        q.x = 0.0f;
        q.y = 0.0f;
        q.z = 0.0f;

        //Quickfix for new AsrObject format
        asr_msgs::AsrObject pObj;
        geometry_msgs::PoseWithCovariance newPose;
        pObj.type = "VitalisSchoko";
        pObj.identifier = "/map";
        pObj.header.frame_id = "/map";
        newPose.pose.position = point;
        newPose.pose.orientation = q;
        pObj.sampledPoses.push_back(newPose);
        objects.push_back(pObj);


        pObj.sampledPoses.clear();
        pObj.type = "Cup";
        pObj.identifier = "/map";
        pObj.header.frame_id = "/map";

        newPose.pose.position = point;
        newPose.pose.orientation = q;

        //pObj.sampledPoses.push_back(newPose);
        //objects.push_back(pObj);
    }


    srv.request.pathToXML = path;
    srv.request.scenes = scenes;
    srv.request.scene_probabilities = probs;
    srv.request.num_votes = atoll(argv[1]);
    srv.request.objects = objects;
    srv.request.base_frame_id = "/map";

    if (client.call(srv))
    {
        for(auto e : srv.response.pose_hypothesis.elements)
        {
            ROS_INFO("Hypothesis type \"%s\" at position (%.2f, %.2f, %.2f), orientation (%.2f, %.2f, %.2f, %.2f), base_frame \"%s\"",
                     e.type.c_str(),
                     e.pose.position.x,
                     e.pose.position.y,
                     e.pose.position.z,
                     e.pose.orientation.w,
                     e.pose.orientation.x,
                     e.pose.orientation.y,
                     e.pose.orientation.z,
                     srv.request.base_frame_id.c_str());
        }
    }
    else
    {
        ROS_ERROR("Failed to call service recognizer_prediction_psm");
        return 1;
    }

    return 0;
}