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(
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
)
......
......@@ -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>
......
#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;
}
......
......@@ -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"
# 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:
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
......
<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>
......@@ -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>
<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 <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;
......
Markdown is supported
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