added tf object broadcaster and generation of simple grasp message

gazebo_state_plugins/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(
   std_msgs
   gazebo_ros
   object_msgs
+  object_msgs_tools
   eigen_conversions
 )
 
@@ -36,7 +37,8 @@ find_package(gazebo REQUIRED)
 catkin_package(
 #  INCLUDE_DIRS include
   LIBRARIES gazebo_state_plugins gazebo_map_publisher
-  CATKIN_DEPENDS geometry_msgs roscpp shape_msgs std_msgs gazebo_ros
+  CATKIN_DEPENDS geometry_msgs roscpp shape_msgs std_msgs gazebo_world_plugin_loader
+        gazebo_ros object_msgs object_msgs_tools eigen_conversions
 #  DEPENDS system_lib
 )
 

gazebo_state_plugins/package.xml

@@ -48,9 +48,11 @@
   <build_depend>shape_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>object_msgs</build_depend>
+  <build_depend>object_msgs_tools</build_depend>
   <build_depend>eigen_conversions</build_depend>
   <run_depend>gazebo_world_plugin_loader</run_depend>
   <run_depend>object_msgs</run_depend>
+  <run_depend>object_msgs_tools</run_depend>
   <run_depend>eigen_conversions</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>roscpp</run_depend>

gazebo_state_plugins/src/GazeboObjectInfo.cpp

 #include <gazebo_state_plugins/GazeboObjectInfo.h>
-
+#include <object_msgs_tools/ObjectFunctions.h>
 #include <gazebo/physics/Link.hh>
 #include <gazebo/physics/BoxShape.hh>
 #include <gazebo/physics/SphereShape.hh>
@@ -60,7 +60,7 @@ bool GazeboObjectInfo::requestObject(object_msgs::ObjectInfo::Request &req, obje
     physics::ModelPtr model=world->GetModel(modelName);
 
     if (!model.get()) {
-        ROS_ERROR("Model %s not found",modelName.c_str());
+        // ROS_ERROR("Model %s not found",modelName.c_str());
         res.success=false;
         return true;
     }
@@ -164,8 +164,8 @@ shape_msgs::SolidPrimitive * GazeboObjectInfo::getSolidPrimitive(physics::Collis
 }
 
 
-GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::ModelPtr& model, bool include_shape){
-
+GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::ModelPtr& model, bool include_shape)
+{
     GazeboObjectInfo::ObjectMsg obj;
 
     physics::Link_V links=model->GetLinks();
@@ -174,21 +174,23 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M
     obj.name=model->GetName();
     obj.header.stamp=ros::Time::now();
     obj.header.frame_id=ROOT_FRAME_ID;
-
-    for (l_it=links.begin(); l_it!=links.end(); ++l_it){
-
+    // obj.type =  no object type given
+    // the custum origin (Object::origin) is going to be set to the first link encountered
+    bool origin_init = false;
+    for (l_it=links.begin(); l_it!=links.end(); ++l_it)
+    {
         physics::LinkPtr link=*l_it;
         
         std::string linkName=link->GetName();
         
-        math::Pose link_pose=link->GetWorldPose ();
+        math::Pose link_pose=link->GetWorldPose();
         //ROS_INFO("Link for model %s: %s, pos %f %f %f",model->GetName().c_str(),link->GetName().c_str(),link_pose.pos.x,link_pose.pos.y,link_pose.pos.z);
         //ROS_INFO("Link found for model %s: %s",model->GetName().c_str(),link->GetName().c_str());
 
         physics::Collision_V colls=link->GetCollisions();
         physics::Collision_V::iterator cit;
-        for (cit=colls.begin(); cit!=colls.end(); ++cit) {
-
+        for (cit=colls.begin(); cit!=colls.end(); ++cit)
+        {
             physics::CollisionPtr c=*cit;
             
             math::Pose rel_pose=c->GetRelativePose();
@@ -208,7 +210,11 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M
             pose.orientation.w=coll_pose.rot.w;
 
             obj.primitive_poses.push_back(pose);
-
+        
+            if (!origin_init)
+            {
+                obj.origin = pose;
+            }
     
             if (include_shape) {
 
@@ -225,9 +231,12 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M
                 obj.content=GazeboObjectInfo::ObjectMsg::POSE;
                 //ROS_INFO("Pub pose for %s (%s %s)",c->GetName().c_str(),link->GetName().c_str(),model->GetName().c_str());
             }
-        
         }
     }
+
+    obj.primitive_origin = GazeboObjectInfo::ObjectMsg::ORIGIN_AVERAGE;
+    obj.mesh_origin = GazeboObjectInfo::ObjectMsg::ORIGIN_UNDEFINED;
+
     return obj;
 }
 

gazebo_test_tools/config/FakeObjectRecognizer.yaml

@@ -13,4 +13,7 @@ publish_recognition_rate: 10
 # can be requested.
 request_object_service: "/gazebo_objects/get_info"
 
-
+# service of under which a object_msgs_tools/RegisterObject can
+# be sent in order to start publishing /tf information about an
+# object.
+register_object_tf_service: "/object_tf_broadcaster/register_object"

gazebo_test_tools/config/ObjectTFBroadcaster.yaml

+# configuration file for publishing object information to /tf, using the same
+# topic names configured for the fake object recogniser.
+# For documentation of the fileds see also $(find object_msgs_tools)/config/ObjectTFBroadcaster.yaml.
+ 
+object_topic: ""
+register_object_service: "/object_tf_broadcaster/register_object"
+object_service: "/gazebo_objects/get_info"
+publish_tf_rate: 100
+query_object_info_rate: 1

gazebo_test_tools/include/gazebo_test_tools/FakeObjectRecognizer.h

@@ -55,7 +55,18 @@ private:
 
     void publishRecognitionEvent(const ros::TimerEvent& e); 
 
-    bool getGazeboObject(const std::string& name, object_msgs::Object& object, bool include_geometry);
+    /**
+     * Like queryObjectInfo but waits a maximum of \e timeout and re-requests
+     * the information every \e checkStep seconds.
+     */
+    bool waitForQueryObjectInfo(const std::string& name, object_msgs::Object& object,
+        bool include_geometry,  float timeout, float checkStep, bool printErrors);
+
+    /**
+     * Uses the object information service to request object information
+     */
+    bool queryObjectInfo(const std::string& name, object_msgs::Object& object,
+        bool include_geometry, bool printErrors);
 
     std::string OBJECTS_TOPIC;
 
@@ -67,12 +78,19 @@ private:
     // an object, of type gazebo_test_tools/TriggerRecognition.srv.
     std::string SERVICE_RECOGNISE_OBJECT_TOPIC;
 
+    
+    // service of under which a object_msgs_tools/RegisterObject can
+    // be sent in order to start publishing /tf information about an
+    // object.
+    std::string SERVICE_REGISTER_OBJECT_TF_TOPIC;
+
     // Recognised objects which are to be continuously published
     // as recognised are published at this rate.
     float PUBLISH_RECOGNISED_OBJECT_RATE;
 
     ros::Publisher object_pub;
     ros::ServiceClient object_info_client;
+    ros::ServiceClient register_object_tf_client;
     
     ros::ServiceServer recognize_object_srv;
 
@@ -83,6 +101,8 @@ private:
     boost::mutex addedObjectsMtx;
 
     ros::Timer publishTimer;
+
+    ros::NodeHandle node;
 };
 
 }  // namespace gazebo_test_tools

gazebo_test_tools/launch/fake_object_recognizer.launch

 <launch>
-
+    # configuration file for the fake object recogniser
     <arg name="config_file" default="$(find gazebo_test_tools)/config/FakeObjectRecognizer.yaml"/>
 
+    # load ROS parameters from configuratoin file
     <rosparam ns="gazebo_test_tools" file="$(arg config_file)"/>
 
 	# name of the node
 	<arg name="node_name" default="fake_object_recognizer"/>
-
+	
+    #####################################
+    # Lauch the fake object recogniser node
+    #####################################
 	<node name="$(arg node_name)" pkg="gazebo_test_tools" type="fake_object_recognizer_node" respawn="true" output="screen"/>
+
+	#####################################
+	# Load the object tf publisher along
+    # with the fake object detector so that
+    # recognised objects also can
+    # get published on tf
+    #####################################
+    <include file="$(find gazebo_test_tools)/launch/object_tf_broadcaster.launch">
+        # use the default config file profided in this package
+        <arg name="config_file" value="$(find gazebo_test_tools)/config/ObjectTFBroadcaster.yaml"/>
+    </include>
 </launch>
 

gazebo_test_tools/launch/gazebo_fake_object_recognition.launch

@@ -9,7 +9,7 @@
 	# Load Gazebo fake object detector 
     #####################################
     <include file="$(find gazebo_test_tools)/launch/fake_object_recognizer.launch">
-        # use the default config file
+        # use the default config file profided in this package
         <arg name="config_file" value="$(find gazebo_test_tools)/config/FakeObjectRecognizer.yaml"/>
     </include>
 </launch>

gazebo_test_tools/launch/object_tf_broadcaster.launch

+<launch>
+    <arg name="config_file" default="$(find gazebo_test_tools)/config/ObjectTFBroadcaster.yaml"/>
+    <include file="$(find object_msgs_tools)/launch/object_tf_broadcaster.launch">
+        <arg name="config_file" value="$(arg config_file)"/>
+    </include>
+</launch>

gazebo_test_tools/launch/spawn_and_recognize_cube.launch

+<launch>
+	<node name="test_cube_spawn" pkg="gazebo_test_tools" type="cube_spawner" respawn="false" output="screen" args="cube1 0 0 1"/>
+	<node name="test_recognise_cube" pkg="gazebo_test_tools" type="fake_object_recognizer_cmd" respawn="false" output="screen" args="cube1 1"/>
+</launch>
+

gazebo_test_tools/src/FakeObjectRecognizer.cpp

 #include <gazebo_test_tools/FakeObjectRecognizer.h>
+#include <object_msgs/RegisterObject.h>
 #include <boost/thread.hpp>
 
 #include <iostream>
@@ -6,6 +7,7 @@
 #define DEFAULT_OBJECTS_TOPIC "world/objects"
 #define DEFAULT_SERVICE_REQUEST_OBJECT_TOPIC "world/request_object"
 #define DEFAULT_SERVICE_RECOGNISE_OBJECT_TOPIC "/recognize_object"
+#define DEFAULT_SERVICE_REGISTER_OBJECT_TF_TOPIC "/register_object"
 #define DEFAULT_PUBLISH_RECOGNISED_OBJECT_RATE 1
 
 using gazebo_test_tools::FakeObjectRecognizer;
@@ -19,24 +21,23 @@ FakeObjectRecognizer::FakeObjectRecognizer() {
     _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());
     
-    SERVICE_RECOGNISE_OBJECT_TOPIC= DEFAULT_SERVICE_RECOGNISE_OBJECT_TOPIC;
-    _node.param<std::string>("recognize_object_service", SERVICE_RECOGNISE_OBJECT_TOPIC,SERVICE_RECOGNISE_OBJECT_TOPIC);
+    _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());
 
-    std::stringstream def_coll_rate;
-    def_coll_rate<<DEFAULT_PUBLISH_RECOGNISED_OBJECT_RATE;    
-    std::string _PUBLISH_RECOGNISED_OBJECT_RATE=def_coll_rate.str();
-    _node.param<std::string>("publish_recognition_rate", _PUBLISH_RECOGNISED_OBJECT_RATE, _PUBLISH_RECOGNISED_OBJECT_RATE);
-    PUBLISH_RECOGNISED_OBJECT_RATE=atof(_PUBLISH_RECOGNISED_OBJECT_RATE.c_str());
+    int PUBLISH_RECOGNISED_OBJECT_RATE;
+    _node.param<int>("publish_recognition_rate", PUBLISH_RECOGNISED_OBJECT_RATE, PUBLISH_RECOGNISED_OBJECT_RATE);
 
-    ros::NodeHandle nodePub("");
-    if (SERVICE_REQUEST_OBJECT_TOPIC!="") object_info_client = nodePub.serviceClient<object_msgs::ObjectInfo>(SERVICE_REQUEST_OBJECT_TOPIC);
+    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);
 
-    recognize_object_srv = nodePub.advertiseService(SERVICE_RECOGNISE_OBJECT_TOPIC, &FakeObjectRecognizer::recognizeObject,this);
+    recognize_object_srv = node.advertiseService(SERVICE_RECOGNISE_OBJECT_TOPIC, &FakeObjectRecognizer::recognizeObject,this);
 
-    object_pub = nodePub.advertise<object_msgs::Object>(OBJECTS_TOPIC, 100); 
+    object_pub = node.advertise<object_msgs::Object>(OBJECTS_TOPIC, 100); 
 
     ros::Rate rate(PUBLISH_RECOGNISED_OBJECT_RATE);
-    publishTimer=nodePub.createTimer(rate,&FakeObjectRecognizer::publishRecognitionEvent, this);
+    publishTimer=node.createTimer(rate,&FakeObjectRecognizer::publishRecognitionEvent, this);
 }
 
 FakeObjectRecognizer::~FakeObjectRecognizer() {
@@ -53,9 +54,9 @@ void FakeObjectRecognizer::publishRecognitionEvent(const ros::TimerEvent& e) {
         // 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.
-        if (!getGazeboObject(*it,object,false))
+        if (!queryObjectInfo(*it,object,false, true))
         {
-            ROS_ERROR_STREAM("Could not find Gazebo object "<<*it);
+            ROS_ERROR_STREAM("Could not find object "<<*it);
             continue;
         }
         object_pub.publish(object);
@@ -65,7 +66,7 @@ void FakeObjectRecognizer::publishRecognitionEvent(const ros::TimerEvent& e) {
 bool FakeObjectRecognizer::recognizeObject(gazebo_test_tools::RecognizeGazeboObject::Request &req,
         gazebo_test_tools::RecognizeGazeboObject::Response &res)
 {
-    ROS_INFO_STREAM("Recognizing gazebo object "<<req.name); 
+    ROS_INFO_STREAM("Recognizing object "<<req.name); 
     
     res.success=true;
     boost::unique_lock<boost::mutex> lock(addedObjectsMtx);
@@ -91,29 +92,58 @@ bool FakeObjectRecognizer::recognizeObject(gazebo_test_tools::RecognizeGazeboObj
     if (req.republish) addedObjects.insert(req.name);
 
     object_msgs::Object object;
-    if (!getGazeboObject(req.name,object,true))
+    // 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))
     {
-        ROS_ERROR_STREAM("Could not find Gazebo object "<<req.name);
+        ROS_ERROR_STREAM("Could not find object "<<req.name);
         res.success=false;
         return true;
     }
 
-    ROS_INFO_STREAM("Publishing object "<<object);       
+    // ROS_INFO_STREAM("Publishing object "<<object);       
     object_pub.publish(object);
+
+    // 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;
+	}
     return true;
 }
 
+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;
+}
 
-bool FakeObjectRecognizer::getGazeboObject(const std::string& name, object_msgs::Object& object, bool include_geometry){
+bool FakeObjectRecognizer::queryObjectInfo(const std::string& name, object_msgs::Object& object, bool include_geometry, bool printErrors){
     object_msgs::ObjectInfo srv;
     srv.request.name=name;
     srv.request.get_geometry=include_geometry;
     if (!object_info_client.call(srv)){
-        ROS_ERROR("Could not get object %s because service request failed.",name.c_str());
+        if (printErrors) ROS_ERROR("Could not get object %s because service request failed.",name.c_str());
         return false;
     }
     if (!srv.response.success) {
-        ROS_ERROR("Could not get object %s because it does not exist in Gazebo.",name.c_str());
+        if (printErrors) ROS_ERROR("Could not get object %s because it does not exist.",name.c_str());
         return false;
     }
     object=srv.response.object;