Commit e4b18b27 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added tf object broadcaster and generation of simple grasp message

parent f662a1da
...@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment