FakeObjectRecognizer.cpp 6.22 KB
Newer Older
Jennifer Buehler's avatar
Jennifer Buehler committed
1
#include <gazebo_test_tools/FakeObjectRecognizer.h>
2
#include <object_msgs/RegisterObject.h>
Jennifer Buehler's avatar
Jennifer Buehler committed
3
4
5
6
7
8
9
#include <boost/thread.hpp>

#include <iostream>

#define DEFAULT_OBJECTS_TOPIC "world/objects"
#define DEFAULT_SERVICE_REQUEST_OBJECT_TOPIC "world/request_object"
#define DEFAULT_SERVICE_RECOGNISE_OBJECT_TOPIC "/recognize_object"
10
#define DEFAULT_SERVICE_REGISTER_OBJECT_TF_TOPIC "/register_object"
Jennifer Buehler's avatar
Jennifer Buehler committed
11
12
13
14
15
16
17
18
19
20
21
22
23
#define DEFAULT_PUBLISH_RECOGNISED_OBJECT_RATE 1

using gazebo_test_tools::FakeObjectRecognizer;

FakeObjectRecognizer::FakeObjectRecognizer() {

    ros::NodeHandle _node("/gazebo_test_tools");
    _node.param<std::string>("objects_topic", OBJECTS_TOPIC, DEFAULT_OBJECTS_TOPIC);
    ROS_INFO("Got objects topic name: <%s>", OBJECTS_TOPIC.c_str());

    _node.param<std::string>("request_object_service", SERVICE_REQUEST_OBJECT_TOPIC, DEFAULT_SERVICE_REQUEST_OBJECT_TOPIC);
    ROS_INFO("Got object service topic name: <%s>", SERVICE_REQUEST_OBJECT_TOPIC.c_str());
    
24
25
26
27
    _node.param<std::string>("recognize_object_service", SERVICE_RECOGNISE_OBJECT_TOPIC, DEFAULT_SERVICE_RECOGNISE_OBJECT_TOPIC);
    
    _node.param<std::string>("register_object_tf_service", SERVICE_REGISTER_OBJECT_TF_TOPIC, DEFAULT_SERVICE_REGISTER_OBJECT_TF_TOPIC);
    ROS_INFO("Got register object tf service topic name: <%s>", SERVICE_REGISTER_OBJECT_TF_TOPIC.c_str());
Jennifer Buehler's avatar
Jennifer Buehler committed
28

29
30
    int PUBLISH_RECOGNISED_OBJECT_RATE;
    _node.param<int>("publish_recognition_rate", PUBLISH_RECOGNISED_OBJECT_RATE, PUBLISH_RECOGNISED_OBJECT_RATE);
Jennifer Buehler's avatar
Jennifer Buehler committed
31

32
33
    if (!SERVICE_REQUEST_OBJECT_TOPIC.empty()) object_info_client = node.serviceClient<object_msgs::ObjectInfo>(SERVICE_REQUEST_OBJECT_TOPIC);
    if (!SERVICE_REGISTER_OBJECT_TF_TOPIC.empty()) register_object_tf_client = node.serviceClient<object_msgs::RegisterObject>(SERVICE_REGISTER_OBJECT_TF_TOPIC);
Jennifer Buehler's avatar
Jennifer Buehler committed
34

35
    recognize_object_srv = node.advertiseService(SERVICE_RECOGNISE_OBJECT_TOPIC, &FakeObjectRecognizer::recognizeObject,this);
Jennifer Buehler's avatar
Jennifer Buehler committed
36

37
    object_pub = node.advertise<object_msgs::Object>(OBJECTS_TOPIC, 100); 
Jennifer Buehler's avatar
Jennifer Buehler committed
38
39

    ros::Rate rate(PUBLISH_RECOGNISED_OBJECT_RATE);
40
    publishTimer=node.createTimer(rate,&FakeObjectRecognizer::publishRecognitionEvent, this);
Jennifer Buehler's avatar
Jennifer Buehler committed
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
}

FakeObjectRecognizer::~FakeObjectRecognizer() {
}

void FakeObjectRecognizer::publishRecognitionEvent(const ros::TimerEvent& e) {
    if (object_pub.getNumSubscribers()==0) return;
    
    boost::unique_lock<boost::mutex> lock(addedObjectsMtx);
    std::set<std::string>::iterator it;
    for (it=addedObjects.begin(); it!=addedObjects.end(); ++it )
    {
        object_msgs::Object object;
        // get the object information. Only the pose is required because
        // the shape has been published in recognizeObject() already and
        // it only needs to be published once when the object is first added.
57
        if (!queryObjectInfo(*it,object,false, true))
Jennifer Buehler's avatar
Jennifer Buehler committed
58
        {
59
            ROS_ERROR_STREAM("Could not find object "<<*it);
Jennifer Buehler's avatar
Jennifer Buehler committed
60
61
62
63
64
65
66
67
68
            continue;
        }
        object_pub.publish(object);
    }
}
    
bool FakeObjectRecognizer::recognizeObject(gazebo_test_tools::RecognizeGazeboObject::Request &req,
        gazebo_test_tools::RecognizeGazeboObject::Response &res)
{
69
    ROS_INFO_STREAM("Recognizing object "<<req.name); 
Jennifer Buehler's avatar
Jennifer Buehler committed
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
    
    res.success=true;
    boost::unique_lock<boost::mutex> lock(addedObjectsMtx);
    std::set<std::string>::iterator it = addedObjects.find(req.name);
    if (it!=addedObjects.end())
    {
        if (req.republish)
        { 
            ROS_WARN_STREAM("The object "<<req.name<<" was already set to being "
                 << "continously published. No need to call FakeObjectRecognizerService again.");
            return true; 
        }
        else  // switch off repbulishing for this object
        {
            ROS_INFO_STREAM("Removing object "<<req.name<<" from being re-published");
            addedObjects.erase(it);
            // now, publish the object information one last time, to
            // make sure this message is not misunderstood: it could have been
            // a caller which wants the message to be published once and wasn't aware
            // that the object was actually being re-published.
        }
    }
    if (req.republish) addedObjects.insert(req.name);

    object_msgs::Object object;
95
96
97
98
99
100
101
    // try this for a while, because sometimes when an object has just been
    // created / spawned, it may take a while for the service to return
    // information about it (before that it can't find it, e.g. if an object
    // is spawned in Gazebo and right after the recognition is triggered)
    float timeout = 3;
    float checkStep = 0.5;
    if (!waitForQueryObjectInfo(req.name,object,true, timeout, checkStep, false))
Jennifer Buehler's avatar
Jennifer Buehler committed
102
    {
103
        ROS_ERROR_STREAM("Could not find object "<<req.name);
Jennifer Buehler's avatar
Jennifer Buehler committed
104
105
106
        res.success=false;
        return true;
    }
Jennifer Buehler's avatar
Jennifer Buehler committed
107

108
    // ROS_INFO_STREAM("Publishing object "<<object);       
Jennifer Buehler's avatar
Jennifer Buehler committed
109
    object_pub.publish(object);
110
111
112
113
114
115
116
117
118

    // now, also register this object to be broadcasted in tf:
    object_msgs::RegisterObject srv;
	srv.request.name = object.name;
	if (register_object_tf_client.call(srv))
	{
		ROS_INFO("FakeObjectRecogniser: Register tf result:");
		std::cout<<srv.response<<std::endl;
	}
Jennifer Buehler's avatar
Jennifer Buehler committed
119
120
121
    return true;
}

122
123
124
125
126
127
128
129
130
131
132
133
134
135
bool FakeObjectRecognizer::waitForQueryObjectInfo(const std::string& name, object_msgs::Object& object,
    bool include_geometry, float timeout, float checkStep, bool printErrors)
{
    ros::Time startTime=ros::Time::now();
    float timeWaited = 0;
    while (timeWaited < timeout)
    {
        if (queryObjectInfo(name, object, include_geometry, printErrors)) return true;
        ros::Duration(checkStep).sleep();
        ros::Time currTime = ros::Time::now();
        timeWaited = (currTime-startTime).toSec();
    }
    return false;
}
Jennifer Buehler's avatar
Jennifer Buehler committed
136

137
bool FakeObjectRecognizer::queryObjectInfo(const std::string& name, object_msgs::Object& object, bool include_geometry, bool printErrors){
Jennifer Buehler's avatar
Jennifer Buehler committed
138
139
140
141
    object_msgs::ObjectInfo srv;
    srv.request.name=name;
    srv.request.get_geometry=include_geometry;
    if (!object_info_client.call(srv)){
142
        if (printErrors) ROS_ERROR("Could not get object %s because service request failed.",name.c_str());
Jennifer Buehler's avatar
Jennifer Buehler committed
143
144
145
        return false;
    }
    if (!srv.response.success) {
146
        if (printErrors) ROS_ERROR("Could not get object %s because it does not exist.",name.c_str());
Jennifer Buehler's avatar
Jennifer Buehler committed
147
148
149
150
151
152
153
        return false;
    }
    object=srv.response.object;
    return true;
}