Skip to content
Snippets Groups Projects
Commit e4b18b27 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added tf object broadcaster and generation of simple grasp message

parent f662a1da
Branches
No related tags found
No related merge requests found
Showing with 140 additions and 39 deletions
...@@ -14,6 +14,7 @@ find_package( ...@@ -14,6 +14,7 @@ find_package(
std_msgs std_msgs
gazebo_ros gazebo_ros
object_msgs object_msgs
object_msgs_tools
eigen_conversions eigen_conversions
) )
...@@ -36,7 +37,8 @@ find_package(gazebo REQUIRED) ...@@ -36,7 +37,8 @@ find_package(gazebo REQUIRED)
catkin_package( catkin_package(
# INCLUDE_DIRS include # INCLUDE_DIRS include
LIBRARIES gazebo_state_plugins gazebo_map_publisher 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 # DEPENDS system_lib
) )
......
...@@ -48,9 +48,11 @@ ...@@ -48,9 +48,11 @@
<build_depend>shape_msgs</build_depend> <build_depend>shape_msgs</build_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>object_msgs</build_depend> <build_depend>object_msgs</build_depend>
<build_depend>object_msgs_tools</build_depend>
<build_depend>eigen_conversions</build_depend> <build_depend>eigen_conversions</build_depend>
<run_depend>gazebo_world_plugin_loader</run_depend> <run_depend>gazebo_world_plugin_loader</run_depend>
<run_depend>object_msgs</run_depend> <run_depend>object_msgs</run_depend>
<run_depend>object_msgs_tools</run_depend>
<run_depend>eigen_conversions</run_depend> <run_depend>eigen_conversions</run_depend>
<run_depend>geometry_msgs</run_depend> <run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend> <run_depend>roscpp</run_depend>
......
#include <gazebo_state_plugins/GazeboObjectInfo.h> #include <gazebo_state_plugins/GazeboObjectInfo.h>
#include <object_msgs_tools/ObjectFunctions.h>
#include <gazebo/physics/Link.hh> #include <gazebo/physics/Link.hh>
#include <gazebo/physics/BoxShape.hh> #include <gazebo/physics/BoxShape.hh>
#include <gazebo/physics/SphereShape.hh> #include <gazebo/physics/SphereShape.hh>
...@@ -60,7 +60,7 @@ bool GazeboObjectInfo::requestObject(object_msgs::ObjectInfo::Request &req, obje ...@@ -60,7 +60,7 @@ bool GazeboObjectInfo::requestObject(object_msgs::ObjectInfo::Request &req, obje
physics::ModelPtr model=world->GetModel(modelName); physics::ModelPtr model=world->GetModel(modelName);
if (!model.get()) { if (!model.get()) {
ROS_ERROR("Model %s not found",modelName.c_str()); // ROS_ERROR("Model %s not found",modelName.c_str());
res.success=false; res.success=false;
return true; return true;
} }
...@@ -164,8 +164,8 @@ shape_msgs::SolidPrimitive * GazeboObjectInfo::getSolidPrimitive(physics::Collis ...@@ -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; GazeboObjectInfo::ObjectMsg obj;
physics::Link_V links=model->GetLinks(); physics::Link_V links=model->GetLinks();
...@@ -174,21 +174,23 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M ...@@ -174,21 +174,23 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M
obj.name=model->GetName(); obj.name=model->GetName();
obj.header.stamp=ros::Time::now(); obj.header.stamp=ros::Time::now();
obj.header.frame_id=ROOT_FRAME_ID; obj.header.frame_id=ROOT_FRAME_ID;
// obj.type = no object type given
for (l_it=links.begin(); l_it!=links.end(); ++l_it){ // 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; physics::LinkPtr link=*l_it;
std::string linkName=link->GetName(); 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 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()); //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 colls=link->GetCollisions();
physics::Collision_V::iterator cit; 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; physics::CollisionPtr c=*cit;
math::Pose rel_pose=c->GetRelativePose(); math::Pose rel_pose=c->GetRelativePose();
...@@ -208,7 +210,11 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M ...@@ -208,7 +210,11 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M
pose.orientation.w=coll_pose.rot.w; pose.orientation.w=coll_pose.rot.w;
obj.primitive_poses.push_back(pose); obj.primitive_poses.push_back(pose);
if (!origin_init)
{
obj.origin = pose;
}
if (include_shape) { if (include_shape) {
...@@ -225,9 +231,12 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M ...@@ -225,9 +231,12 @@ GazeboObjectInfo::ObjectMsg GazeboObjectInfo::createBoundingBoxObject(physics::M
obj.content=GazeboObjectInfo::ObjectMsg::POSE; 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()); //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; return obj;
} }
......
...@@ -13,4 +13,7 @@ publish_recognition_rate: 10 ...@@ -13,4 +13,7 @@ publish_recognition_rate: 10
# can be requested. # can be requested.
request_object_service: "/gazebo_objects/get_info" 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"
# 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
...@@ -55,7 +55,18 @@ private: ...@@ -55,7 +55,18 @@ private:
void publishRecognitionEvent(const ros::TimerEvent& e); 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; std::string OBJECTS_TOPIC;
...@@ -67,12 +78,19 @@ private: ...@@ -67,12 +78,19 @@ private:
// an object, of type gazebo_test_tools/TriggerRecognition.srv. // an object, of type gazebo_test_tools/TriggerRecognition.srv.
std::string SERVICE_RECOGNISE_OBJECT_TOPIC; 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 // Recognised objects which are to be continuously published
// as recognised are published at this rate. // as recognised are published at this rate.
float PUBLISH_RECOGNISED_OBJECT_RATE; float PUBLISH_RECOGNISED_OBJECT_RATE;
ros::Publisher object_pub; ros::Publisher object_pub;
ros::ServiceClient object_info_client; ros::ServiceClient object_info_client;
ros::ServiceClient register_object_tf_client;
ros::ServiceServer recognize_object_srv; ros::ServiceServer recognize_object_srv;
...@@ -83,6 +101,8 @@ private: ...@@ -83,6 +101,8 @@ private:
boost::mutex addedObjectsMtx; boost::mutex addedObjectsMtx;
ros::Timer publishTimer; ros::Timer publishTimer;
ros::NodeHandle node;
}; };
} // namespace gazebo_test_tools } // namespace gazebo_test_tools
......
<launch> <launch>
# configuration file for the fake object recogniser
<arg name="config_file" default="$(find gazebo_test_tools)/config/FakeObjectRecognizer.yaml"/> <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)"/> <rosparam ns="gazebo_test_tools" file="$(arg config_file)"/>
# name of the node # name of the node
<arg name="node_name" default="fake_object_recognizer"/> <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"/> <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> </launch>
...@@ -9,7 +9,7 @@ ...@@ -9,7 +9,7 @@
# Load Gazebo fake object detector # Load Gazebo fake object detector
##################################### #####################################
<include file="$(find gazebo_test_tools)/launch/fake_object_recognizer.launch"> <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"/> <arg name="config_file" value="$(find gazebo_test_tools)/config/FakeObjectRecognizer.yaml"/>
</include> </include>
</launch> </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>
<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>
#include <gazebo_test_tools/FakeObjectRecognizer.h> #include <gazebo_test_tools/FakeObjectRecognizer.h>
#include <object_msgs/RegisterObject.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <iostream> #include <iostream>
...@@ -6,6 +7,7 @@ ...@@ -6,6 +7,7 @@
#define DEFAULT_OBJECTS_TOPIC "world/objects" #define DEFAULT_OBJECTS_TOPIC "world/objects"
#define DEFAULT_SERVICE_REQUEST_OBJECT_TOPIC "world/request_object" #define DEFAULT_SERVICE_REQUEST_OBJECT_TOPIC "world/request_object"
#define DEFAULT_SERVICE_RECOGNISE_OBJECT_TOPIC "/recognize_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 #define DEFAULT_PUBLISH_RECOGNISED_OBJECT_RATE 1
using gazebo_test_tools::FakeObjectRecognizer; using gazebo_test_tools::FakeObjectRecognizer;
...@@ -19,24 +21,23 @@ FakeObjectRecognizer::FakeObjectRecognizer() { ...@@ -19,24 +21,23 @@ FakeObjectRecognizer::FakeObjectRecognizer() {
_node.param<std::string>("request_object_service", SERVICE_REQUEST_OBJECT_TOPIC, DEFAULT_SERVICE_REQUEST_OBJECT_TOPIC); _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()); 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, DEFAULT_SERVICE_RECOGNISE_OBJECT_TOPIC);
_node.param<std::string>("recognize_object_service", SERVICE_RECOGNISE_OBJECT_TOPIC,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; int PUBLISH_RECOGNISED_OBJECT_RATE;
def_coll_rate<<DEFAULT_PUBLISH_RECOGNISED_OBJECT_RATE; _node.param<int>("publish_recognition_rate", PUBLISH_RECOGNISED_OBJECT_RATE, 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());
ros::NodeHandle nodePub(""); if (!SERVICE_REQUEST_OBJECT_TOPIC.empty()) object_info_client = node.serviceClient<object_msgs::ObjectInfo>(SERVICE_REQUEST_OBJECT_TOPIC);
if (SERVICE_REQUEST_OBJECT_TOPIC!="") object_info_client = nodePub.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); ros::Rate rate(PUBLISH_RECOGNISED_OBJECT_RATE);
publishTimer=nodePub.createTimer(rate,&FakeObjectRecognizer::publishRecognitionEvent, this); publishTimer=node.createTimer(rate,&FakeObjectRecognizer::publishRecognitionEvent, this);
} }
FakeObjectRecognizer::~FakeObjectRecognizer() { FakeObjectRecognizer::~FakeObjectRecognizer() {
...@@ -53,9 +54,9 @@ void FakeObjectRecognizer::publishRecognitionEvent(const ros::TimerEvent& e) { ...@@ -53,9 +54,9 @@ void FakeObjectRecognizer::publishRecognitionEvent(const ros::TimerEvent& e) {
// get the object information. Only the pose is required because // get the object information. Only the pose is required because
// the shape has been published in recognizeObject() already and // the shape has been published in recognizeObject() already and
// it only needs to be published once when the object is first added. // 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; continue;
} }
object_pub.publish(object); object_pub.publish(object);
...@@ -65,7 +66,7 @@ void FakeObjectRecognizer::publishRecognitionEvent(const ros::TimerEvent& e) { ...@@ -65,7 +66,7 @@ void FakeObjectRecognizer::publishRecognitionEvent(const ros::TimerEvent& e) {
bool FakeObjectRecognizer::recognizeObject(gazebo_test_tools::RecognizeGazeboObject::Request &req, bool FakeObjectRecognizer::recognizeObject(gazebo_test_tools::RecognizeGazeboObject::Request &req,
gazebo_test_tools::RecognizeGazeboObject::Response &res) gazebo_test_tools::RecognizeGazeboObject::Response &res)
{ {
ROS_INFO_STREAM("Recognizing gazebo object "<<req.name); ROS_INFO_STREAM("Recognizing object "<<req.name);
res.success=true; res.success=true;
boost::unique_lock<boost::mutex> lock(addedObjectsMtx); boost::unique_lock<boost::mutex> lock(addedObjectsMtx);
...@@ -91,29 +92,58 @@ bool FakeObjectRecognizer::recognizeObject(gazebo_test_tools::RecognizeGazeboObj ...@@ -91,29 +92,58 @@ bool FakeObjectRecognizer::recognizeObject(gazebo_test_tools::RecognizeGazeboObj
if (req.republish) addedObjects.insert(req.name); if (req.republish) addedObjects.insert(req.name);
object_msgs::Object object; 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; res.success=false;
return true; return true;
} }
ROS_INFO_STREAM("Publishing object "<<object); // ROS_INFO_STREAM("Publishing object "<<object);
object_pub.publish(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; 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; object_msgs::ObjectInfo srv;
srv.request.name=name; srv.request.name=name;
srv.request.get_geometry=include_geometry; srv.request.get_geometry=include_geometry;
if (!object_info_client.call(srv)){ 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; return false;
} }
if (!srv.response.success) { 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; return false;
} }
object=srv.response.object; object=srv.response.object;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment