def run (self):
coll_obj = CollisionObject()
coll_obj.operation.operation = CollisionObjectOperation.ADD
coll_obj.id = self.name_of_the_target_object
coll_obj.header = rospy.Header()
coll_obj.header.frame_id = self.frame_id
coll_obj.poses = [self.pose_of_the_target_object]
coll_obj.shapes = [self.shape]
coll_obj.padding = self.padding
pub = rospy.Publisher('/collision_object',CollisionObject,latch=True)
rospy.loginfo('Thread publishing on /collision_object topic (%s)',self.name_of_the_target_object)
while (rospy.is_shutdown() == False) and (self.end == False):
coll_obj.header.stamp = rospy.Time.now()
try:
pub.publish(coll_obj)
except Exception, e:
rospy.logerr("Error on publishing to /collision_object topic: %s", e)
return 0
rospy.logdebug('Thread is looping')
rospy.sleep(2)
def process_collision_geometry_for_cluster(self, cluster):
rospy.loginfo("adding cluster with %d points to collision map"%len(cluster.points))
many_boxes = CollisionObject()
many_boxes.operation.operation = CollisionObjectOperation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points)/100.0)
random_indices = range(len(cluster.points))
scipy.random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005]*3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0,0,0,1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
collision_name = self.get_next_object_name()
many_boxes.id = collision_name
self.object_in_map_pub.publish(many_boxes)
return collision_name
def add_collision_cluster(self, cluster, object_id):
'''
Adds a point cloud to the collision map as a single collision object composed of many small boxes.
**Args:**
**cluster (sensor_msg.msg.PointCloud):** The point cloud to add to the map
**object_id (string):** The name the point cloud should have in the map
'''
many_boxes = CollisionObject()
many_boxes.operation.operation = many_boxes.operation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points)/100.0)
random_indices = range(len(cluster.points))
random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005]*3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0,0,0,1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
many_boxes.id = object_id
self._publish(many_boxes, self._collision_object_pub)
def process_collision_geometry_for_cluster(self, cluster):
rospy.loginfo("adding cluster with %d points to collision map" %
len(cluster.points))
many_boxes = CollisionObject()
many_boxes.operation.operation = CollisionObjectOperation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points) / 100.0)
random_indices = range(len(cluster.points))
scipy.random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005] * 3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0, 0, 0, 1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
collision_name = self.get_next_object_name()
many_boxes.id = collision_name
self.object_in_map_pub.publish(many_boxes)
return collision_name
def add_collision_cluster(self, cluster, object_id):
'''
Adds a point cloud to the collision map as a single collision object composed of many small boxes.
**Args:**
**cluster (sensor_msg.msg.PointCloud):** The point cloud to add to the map
**object_id (string):** The name the point cloud should have in the map
'''
many_boxes = CollisionObject()
many_boxes.operation.operation = many_boxes.operation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points) / 100.0)
random_indices = range(len(cluster.points))
random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005] * 3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0, 0, 0, 1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
many_boxes.id = object_id
self._publish(many_boxes, self._collision_object_pub)
def _add_table_to_map(self, table, table_name):
co = CollisionObject()
co.id = table_name
if not co.id:
co.id = 'current_table'
co.header = table.pose.header
#we do NOT want a padded table
co.padding = -1
if len(table.convex_hull.vertices) > 0:
if self.table_thickness > 0:
table_z = range(0, int(1000*self.table_thickness/2.0), int(1000*TABLE_RESOLUTION))
table_z.append(1000.0*self.table_thickness/2.0)
sgnrng = [1.0, -1.0]
else:
table_z = [0]
sgnrng = [1.0]
for z in table_z:
for sgn in sgnrng:
co.shapes.append(table.convex_hull)
ps = tl.transform_pose_stamped(self._wi.world_frame, table.pose)
ps.pose.position.z += sgn*z/1000.0
co.poses.append(tl.transform_pose(table.pose.header.frame_id, ps.header.frame_id, ps.pose))
rospy.logdebug('Adding table as convex hull')
else:
bbox = Shape()
bbox.type = bbox.BOX
bbox.dimensions = [abs(table.x_max - table.x_min), abs(table.y_max - table.y_min), self.table_thickness]
co.shapes.append(bbox)
co.poses.append(table.pose.pose)
rospy.logdebug('Adding table as bounding box')
self._wi.add_object(co)
return co.id
def add_map_tables(wi):
(serving_table, dirty_table) = [add_map_table('serving_table', wi), add_map_table('dirty_table', wi)]
#little_table = add_map_table('little_table', wi)
st_base = CollisionObject()
st_base.header = serving_table.pose.header
st_base.id = "serving_table_base"
st_base.poses.append(copy.deepcopy(serving_table.pose.pose))
st_base.poses[0].position.z = 0.1
st_shape = Shape()
st_shape.type = Shape.CYLINDER
st_shape.dimensions = [0.3, 0.1]
st_base.shapes.append(st_shape)
wi.add_object(st_base)
def _get_bounding_box_collision_object(self, box):
#we don't try to extend the box down to the table...
#so hopefully we detected all the way down
co = CollisionObject()
co.header = box.pose.header
shape = Shape()
shape.type = shape.BOX
shape.dimensions.append(box.box_dims.x)
shape.dimensions.append(box.box_dims.y)
shape.dimensions.append(box.box_dims.z)
co.shapes.append(shape)
co.poses.append(box.pose.pose)
return co
def add_collision_box(self, box_pose_stamped, box_dims, object_id):
'''
Adds a box to the map as a collision object.
**Args:**
**box_pose_stamped (geometry_msgs.msg.PoseStamped):** The pose of the box.
**box_dims (tuple of 3 doubles):** The dimensions of the box as (x_dimension, y_dimension, z_dimension)
**object_id (string):** The ID the box should have in the collision map
'''
box = CollisionObject()
box.operation.operation = box.operation.ADD
box.header = box_pose_stamped.header
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = box_dims
box.shapes.append(shape)
box.poses.append(box_pose_stamped.pose)
box.id = object_id
self._publish(box, self._collision_object_pub)
return box
def add_collision_box(self, box_pose_stamped, box_dims, object_id):
'''
Adds a box to the map as a collision object.
**Args:**
**box_pose_stamped (geometry_msgs.msg.PoseStamped):** The pose of the box.
**box_dims (tuple of 3 doubles):** The dimensions of the box as (x_dimension, y_dimension, z_dimension)
**object_id (string):** The ID the box should have in the collision map
'''
box = CollisionObject()
box.operation.operation = box.operation.ADD
box.header = box_pose_stamped.header
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = box_dims
box.shapes.append(shape)
box.poses.append(box_pose_stamped.pose)
box.id = object_id
self._publish(box, self._collision_object_pub)
return box
def detect_objects(self, add_objects_to_map='all', add_table_to_map=True,
add_objects_as_mesh='all', reset_collision_objects=True,
table_name='', point_head_at=None, reset_collider=True,
labels_callback=None):
'''
This is the function you should call to get detections of objects in the scene. By default, it will
give you back a TablewareResult, remove all collision objects (although not attached objects) currently in
the collision map and add the objects detected and the table to the map. This returns first pickup goals for
objects recognized as tableware, then for objects recognized but not as tableware and then for objects not
recognized at all. In order to use the pickup goals returned, you must fill in the arm_name field.
The object labels returned in pickup goal reflect the recognition result. Labels are:
For tableware: One of the keys in the tableware_labels list.
For recognized objects that are not tableware: The first tag in the database
For unrecognized objects: graspable
**Args:**
*add_objects_to_map (string or False):* This argument controls whether and which objects are added to the
collision map during detection. Passing 'all' will add all detected objects to map, 'recognized'
will add only recognized objects to map, 'tableware' will add only objects recognized as tableware,
and False or 'None' or 'none' will add nothing to the map.
*add_table_to_map (boolean):* If true, this will add the detected table to the map. If you do not already
have a representation of the table in the map and you wish to do pick or place it is a good idea to
have the detection add it so there is a defined region of the map in which collision checking for the
object can be disabled. If the table is added to the map, the collision_support_surface_id field of
the pickup goals will be set to its collision id. Tables are usually added as meshes although
if only the bounding box for the table is returned they will be added as boxes.
*add_objects_as_mesh ('all', False, or [string]):* This argument controls which objects are added
to the map as mesh rather than as a bounding box. 'all' will add all recognized objects as mesh while
False will add no recognized objects as mesh. For finer control, you can pass a list of labels that
you want to add as mesh.
*reset_collision_objects (boolean):* If true, this will remove all collision objects from the map before
adding new ones. The detection does no correlation with what is currently in the map so if you have
done a detection on this table before, you will get repeat objects added to the map if you do not
clear them first.
*table_name (string):* The name of the table on which the objects are sitting. If passed in, this will be
added to the pickup goals to avoid extraneous collisions caused by the object contacting the table
(and will also be the table_name in the result). If the table is added to the collision
map and this is passed in, the table with have this name in the collision map. If the table is added
to the collision map and this is not passed in, it will have the name 'current_table', which is
returned by the TablewareResult.
*point_head_at (geometry_msgs.msg.PointStamped):* A point at which the head should look for the detection.
If passed in, the robot will first look at this point, but if it cannot find a table or finds a
vertical table, it will search along the x axis for a table. For this reason, it is always a good
idea to pass in this argument. If left at None, the detection will not move the head.
*reset_collider (boolean):* True if you want the collider to be reset before detection. This will not
wait for repopulate before the detection but will delay the return from the detection until after the
repopulate is done if the detection is quick.
*labels_callback (function: string f(pr2_tasks.pickplace_definitions.PickupGoal,
tabletop_object_detector.msgs.Table, string)):* After do_detection is finished assigning a label to a
cluster, this function (if not None) will be called to do more post-processing on the cluster. For
example, this function is used to identify utensils by checking the height and pose of any
unidentified cluster. It should return the label of the cluster.
**Returns:**
A TablewareDetectionResult. The pickup goals (see pickplace_definitions.py) in this result have all of the
detection information, including recognized models and point cloud information, in the GraspableObject
field. The pose at which the object was detected and the label are also returned as fields in the
pickup goals.
**Raises:**
**exceptions.DetectionError:** if a table is not found or only vertical tables cannot be found. If only
vertical tables were found the error code will be OTHER_ERROR.
'''
if reset_collision_objects:
self._wi.reset_collision_objects()
if reset_collider:
#this function takes so long that we'll have repopulated by the end of it
self._wi.reset_collider_node(repopulate=False)
reset_time = rospy.Time.now()
collision_objects = self._wi.collision_objects()
attached_objects = self._wi.attached_collision_objects()
used_names = []
for o in collision_objects:
used_names.append(o.id)
for ao in attached_objects:
used_names.append(ao.object.id)
res = self._do_detection(point_head_at)
if add_table_to_map:
table_name = self._add_table_to_map(res.detection.table, table_name)
#we want to return first the tableware objects we might want to pick up
tableware_pgs = []
#recognized means we recognized them but don't think
#they are anything we should pick up
recognized_pgs = []
#graspable means we could pick them up but we don't
#know what they are
graspable_pgs = []
for c in range(len(res.detection.clusters)):
#Compute the bounding box
try:
bres = self._bounding_box_srv(res.detection.clusters[c])
except rospy.ServiceException, e:
rospy.logerr('Unable to call cluster bounding box service. Exception was '+str(e)+
'. Ignoring cluster.')
continue
if bres.error_code != bres.SUCCESS:
rospy.logerr('Cluster bounding box service returned error '+str(bres.error_code)+
'. Ignoring cluster')
continue
go = GraspableObject()
go.reference_frame_id = res.detection.clusters[c].header.frame_id
go.cluster = res.detection.clusters[c]
pg = PickupGoal('', go, object_pose_stamped=bres.pose,
collision_support_surface_name=table_name, allow_gripper_support_collision=True)
co = CollisionObject()
label = 'graspable'
if self.get_model_description and self.get_mesh_from_database and\
len(res.detection.models[c].model_list) > 0 and\
res.detection.models[c].model_list[0].confidence < 0.005:
#we recognized this - figure out what it is
model_id = res.detection.models[c].model_list[0].model_id
go.potential_models = res.detection.models[c].model_list
rospy.logdebug('Potential models are: '+ str([m.model_id for m in res.detection.models[c].model_list]))
try:
descr = self.get_model_description(model_id)
if descr.return_code.code != descr.return_code.SUCCESS:
rospy.logwarn('Get model description returned error '+
str(descr.return_code.code))
for t in descr.tags:
if t in self.tableware_labels:
label = t
break
if label == 'graspable' and descr.tags:
label = descr.tags[0]
rospy.logdebug('Name of model is '+descr.name)
except rospy.ServiceException, e:
rospy.logerr('Call to get description threw exception '+str(e))
shape = None
if add_objects_as_mesh and (add_objects_as_mesh == 'all' or label in add_objects_as_mesh):
try:
shape = self.get_mesh_from_database(res.detection.models[c].model_list[0].model_id)
if label == 'graspable': label = 'recognized'
co.header = res.detection.models[c].model_list[0].pose.header
co.shapes.append(shape)
co.poses.append(res.detection.models[c].model_list[0].pose.pose)
pg.object_pose_stamped = tl.transform_pose_stamped(pg.target.reference_frame_id,
res.detection.models[c].model_list[0].pose)
except DetectionError:
shape = None
if not shape:
co = self._get_bounding_box_collision_object(bres)
pg.object_pose_stamped = tl.transform_pose_stamped(pg.target.reference_frame_id, bres.pose)
