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 _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