From e4b18b275f2299a898e15c8ee0a3a79eaf7037e2 Mon Sep 17 00:00:00 2001
From: Jennifer Buehler <jennifer.e.buehler@gmail.com>
Date: Sun, 6 Mar 2016 23:28:20 +0100
Subject: [PATCH] added tf object broadcaster and generation of simple grasp
message
---
gazebo_state_plugins/CMakeLists.txt | 4 +-
gazebo_state_plugins/package.xml | 2 +
gazebo_state_plugins/src/GazeboObjectInfo.cpp | 33 +++++----
.../config/FakeObjectRecognizer.yaml | 5 +-
.../config/ObjectTFBroadcaster.yaml | 9 +++
.../gazebo_test_tools/FakeObjectRecognizer.h | 22 +++++-
.../launch/fake_object_recognizer.launch | 19 ++++-
.../gazebo_fake_object_recognition.launch | 2 +-
.../launch/object_tf_broadcaster.launch | 6 ++
.../launch/spawn_and_recognize_cube.launch | 5 ++
.../src/FakeObjectRecognizer.cpp | 72 +++++++++++++------
11 files changed, 140 insertions(+), 39 deletions(-)
create mode 100644 gazebo_test_tools/config/ObjectTFBroadcaster.yaml
create mode 100644 gazebo_test_tools/launch/object_tf_broadcaster.launch
create mode 100644 gazebo_test_tools/launch/spawn_and_recognize_cube.launch
diff --git a/gazebo_state_plugins/CMakeLists.txt b/gazebo_state_plugins/CMakeLists.txt
index 6557029..a03c3aa 100644
--- a/gazebo_state_plugins/CMakeLists.txt
+++ b/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
)
diff --git a/gazebo_state_plugins/package.xml b/gazebo_state_plugins/package.xml
index 062325a..50c398a 100644
--- a/gazebo_state_plugins/package.xml
+++ b/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>
diff --git a/gazebo_state_plugins/src/GazeboObjectInfo.cpp b/gazebo_state_plugins/src/GazeboObjectInfo.cpp
index f1eed13..5ce7730 100644
--- a/gazebo_state_plugins/src/GazeboObjectInfo.cpp
+++ b/gazebo_state_plugins/src/GazeboObjectInfo.cpp
@@ -1,5 +1,5 @@
#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;
}
diff --git a/gazebo_test_tools/config/FakeObjectRecognizer.yaml b/gazebo_test_tools/config/FakeObjectRecognizer.yaml
index d0a5ff7..ee42bf6 100644
--- a/gazebo_test_tools/config/FakeObjectRecognizer.yaml
+++ b/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"
diff --git a/gazebo_test_tools/config/ObjectTFBroadcaster.yaml b/gazebo_test_tools/config/ObjectTFBroadcaster.yaml
new file mode 100644
index 0000000..03c9a57
--- /dev/null
+++ b/gazebo_test_tools/config/ObjectTFBroadcaster.yaml
@@ -0,0 +1,9 @@
+# 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
diff --git a/gazebo_test_tools/include/gazebo_test_tools/FakeObjectRecognizer.h b/gazebo_test_tools/include/gazebo_test_tools/FakeObjectRecognizer.h
index bc2889b..1774cff 100644
--- a/gazebo_test_tools/include/gazebo_test_tools/FakeObjectRecognizer.h
+++ b/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
diff --git a/gazebo_test_tools/launch/fake_object_recognizer.launch b/gazebo_test_tools/launch/fake_object_recognizer.launch
index 402a836..e7516c1 100644
--- a/gazebo_test_tools/launch/fake_object_recognizer.launch
+++ b/gazebo_test_tools/launch/fake_object_recognizer.launch
@@ -1,12 +1,27 @@
<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>
diff --git a/gazebo_test_tools/launch/gazebo_fake_object_recognition.launch b/gazebo_test_tools/launch/gazebo_fake_object_recognition.launch
index 8865f4c..1d1eb72 100644
--- a/gazebo_test_tools/launch/gazebo_fake_object_recognition.launch
+++ b/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>
diff --git a/gazebo_test_tools/launch/object_tf_broadcaster.launch b/gazebo_test_tools/launch/object_tf_broadcaster.launch
new file mode 100644
index 0000000..85ecf35
--- /dev/null
+++ b/gazebo_test_tools/launch/object_tf_broadcaster.launch
@@ -0,0 +1,6 @@
+<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>
diff --git a/gazebo_test_tools/launch/spawn_and_recognize_cube.launch b/gazebo_test_tools/launch/spawn_and_recognize_cube.launch
new file mode 100644
index 0000000..8a79ff3
--- /dev/null
+++ b/gazebo_test_tools/launch/spawn_and_recognize_cube.launch
@@ -0,0 +1,5 @@
+<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>
+
diff --git a/gazebo_test_tools/src/FakeObjectRecognizer.cpp b/gazebo_test_tools/src/FakeObjectRecognizer.cpp
index 4e3c964..3fec06a 100644
--- a/gazebo_test_tools/src/FakeObjectRecognizer.cpp
+++ b/gazebo_test_tools/src/FakeObjectRecognizer.cpp
@@ -1,4 +1,5 @@
#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;
--
GitLab