Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
CeTI
ROS
ROS Packages
gazebo-pkgs
Commits
e4b18b27
Commit
e4b18b27
authored
Mar 06, 2016
by
Jennifer Buehler
Browse files
added tf object broadcaster and generation of simple grasp message
parent
f662a1da
Changes
11
Hide whitespace changes
Inline
Side-by-side
gazebo_state_plugins/CMakeLists.txt
View file @
e4b18b27
...
...
@@ -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
View file @
e4b18b27
...
...
@@ -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
View file @
e4b18b27
#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
View file @
e4b18b27
...
...
@@ -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
0 → 100644
View file @
e4b18b27
# 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
View file @
e4b18b27
...
...
@@ -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
View file @
e4b18b27
<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
View file @
e4b18b27
...
...
@@ -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
0 → 100644
View file @
e4b18b27
<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
0 → 100644
View file @
e4b18b27
<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
View file @
e4b18b27
#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_RE
QUEST
_OBJECT_TOPIC
!=
""
)
object_
info
_client
=
node
Pub
.
serviceClient
<
object_msgs
::
Object
Info
>
(
SERVICE_RE
QUEST
_OBJECT_TOPIC
);
if
(
!
SERVICE_REQUEST_OBJECT_TOPIC
.
empty
())
object_info_client
=
node
.
serviceClient
<
object_msgs
::
ObjectInfo
>
(
SERVICE_REQUEST_OBJECT_TOPIC
);
if
(
!
SERVICE_RE
GISTER
_OBJECT_
TF_
TOPIC
.
empty
())
register_
object_
tf
_client
=
node
.
serviceClient
<
object_msgs
::
Register
Object
>
(
SERVICE_RE
GISTER
_OBJECT_
TF_
TOPIC
);
recognize_object_srv
=
node
Pub
.
advertiseService
(
SERVICE_RECOGNISE_OBJECT_TOPIC
,
&
FakeObjectRecognizer
::
recognizeObject
,
this
);
recognize_object_srv
=
node
.
advertiseService
(
SERVICE_RECOGNISE_OBJECT_TOPIC
,
&
FakeObjectRecognizer
::
recognizeObject
,
this
);
object_pub
=
node
Pub
.
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
=
node
Pub
.
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
(
!
getGazebo
Object
(
*
it
,
object
,
false
))
if
(
!
query
Object
Info
(
*
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
::
getGazebo
Object
(
const
std
::
string
&
name
,
object_msgs
::
Object
&
object
,
bool
include_geometry
){
bool
FakeObjectRecognizer
::
query
Object
Info
(
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
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment