def _get_reset_object(self):
     reset_object = CollisionObject()
     reset_object.operation.operation = reset_object.operation.REMOVE
     reset_object.header.frame_id = 'base_link'
     reset_object.header.stamp = rospy.Time.now()
     reset_object.id = 'all'
     return reset_object
    def setUp(self):

        rospy.init_node('test_motion_execution_buffer')
        
        self.tf = TransformListener()        

        self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
        
        self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)

        self.move_arm_action_client.wait_for_server()

        obj1 = CollisionObject()
    
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "obj1";
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(1)]
        obj1.shapes[0].type = Shape.CYLINDER
        obj1.shapes[0].dimensions = [float() for _ in range(2)]
        obj1.shapes[0].dimensions[0] = .1
        obj1.shapes[0].dimensions[1] = 1.5
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = .6
        obj1.poses[0].position.y = -.6
        obj1.poses[0].position.z = .75
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1
        
        self.obj_pub.publish(obj1)

        rospy.sleep(2.0)
    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_table(self, firsttable, additional_tables = []):

        table_object = CollisionObject()
        table_object.operation.operation = CollisionObjectOperation.ADD
        table_object.header.frame_id = firsttable.pose.header.frame_id
        table_object.header.stamp = rospy.Time.now()

        #create a box for each table
        for table in [firsttable,]+additional_tables:
            object = Shape()
            object.type = Shape.BOX;
            object.dimensions.append(math.fabs(table.x_max-table.x_min))
            object.dimensions.append(math.fabs(table.y_max-table.y_min))
            object.dimensions.append(0.01)
            table_object.shapes.append(object)
  
        #set the origin of the table object in the middle of the firsttable
        table_mat = self.pose_to_mat(firsttable.pose.pose)
        table_offset = scipy.matrix([(firsttable.x_min + firsttable.x_max)/2.0, (firsttable.y_min + firsttable.y_max)/2.0, 0.0]).T
        table_offset_mat = scipy.matrix(scipy.identity(4))
        table_offset_mat[0:3,3] = table_offset
        table_center = table_mat * table_offset_mat
        origin_pose = self.mat_to_pose(table_center)
        table_object.poses.append(origin_pose)

        table_object.id = "table"
        self.object_in_map_pub.publish(table_object)
Exemple #5
0
    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)
Exemple #6
0
 def _get_reset_object(self):
     reset_object = CollisionObject()
     reset_object.operation.operation = reset_object.operation.REMOVE
     reset_object.header.frame_id = 'base_link'
     reset_object.header.stamp = rospy.Time.now()
     reset_object.id = 'all'
     return reset_object
    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_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 process_collision_geometry_for_table(self,
                                             firsttable,
                                             additional_tables=[]):

        table_object = CollisionObject()
        table_object.operation.operation = CollisionObjectOperation.ADD
        table_object.header.frame_id = firsttable.pose.header.frame_id
        table_object.header.stamp = rospy.Time.now()

        #create a box for each table
        for table in [
                firsttable,
        ] + additional_tables:
            object = Shape()
            object.type = Shape.BOX
            object.dimensions.append(math.fabs(table.x_max - table.x_min))
            object.dimensions.append(math.fabs(table.y_max - table.y_min))
            object.dimensions.append(0.01)
            table_object.shapes.append(object)

        #set the origin of the table object in the middle of the firsttable
        table_mat = self.pose_to_mat(firsttable.pose.pose)
        table_offset = scipy.matrix([
            (firsttable.x_min + firsttable.x_max) / 2.0,
            (firsttable.y_min + firsttable.y_max) / 2.0, 0.0
        ]).T
        table_offset_mat = scipy.matrix(scipy.identity(4))
        table_offset_mat[0:3, 3] = table_offset
        table_center = table_mat * table_offset_mat
        origin_pose = self.mat_to_pose(table_center)
        table_object.poses.append(origin_pose)

        table_object.id = "table"
        self.object_in_map_pub.publish(table_object)
    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 remove_collision_object(self, collision_name):

        reset_object = CollisionObject()
        reset_object.operation.operation = CollisionObjectOperation.REMOVE
        reset_object.header.frame_id = "base_link"
        reset_object.header.stamp = rospy.Time.now()
        reset_object.id = collision_name
        self.object_in_map_pub.publish(reset_object) 
    def remove_collision_object(self, collision_name):

        reset_object = CollisionObject()
        reset_object.operation.operation = CollisionObjectOperation.REMOVE
        reset_object.header.frame_id = "base_link"
        reset_object.header.stamp = rospy.Time.now()
        reset_object.id = collision_name
        self.object_in_map_pub.publish(reset_object)
    def tearDown(self):
        obj1 = CollisionObject()
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "all"
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.REMOVE

        self.obj_pub.publish(obj1)

        rospy.sleep(2.0)
    def tearDown(self):
        obj1 = CollisionObject()
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "all";
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.REMOVE

        self.obj_pub.publish(obj1)
        
        rospy.sleep(2.0)
            def reset4_request_cb(userdata, request):
            	print "Creating empty request for environment_server"
                empty_request = GetPlanningSceneRequest()
                # I dont think this is needed anymore but as its working i dont want to touch it
                col_obj = CollisionObject()
                col_obj.operation.operation = CollisionObjectOperation.REMOVE
                col_obj.id = "table"  # die you bastard

                empty_request.planning_scene_diff.collision_objects.append(col_obj)
                return empty_request
Exemple #16
0
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, box_dims, frame_id, collision_name):
     
     rospy.loginfo("adding box to collision map")
     box = CollisionObject()
     box.operation.operation = CollisionObjectOperation.ADD
     box.header.frame_id = frame_id
     box.header.stamp = rospy.Time.now()
     shape = Shape()
     shape.type = Shape.BOX
     shape.dimensions = box_dims
     box.shapes.append(shape)
     box.poses.append(box_pose)
     box.id = collision_name
     self.object_in_map_pub.publish(box)
    def add_collision_box(self, box_pose, box_dims, frame_id, collision_name):

        rospy.loginfo("adding box to collision map")
        box = CollisionObject()
        box.operation.operation = CollisionObjectOperation.ADD
        box.header.frame_id = frame_id
        box.header.stamp = rospy.Time.now()
        shape = Shape()
        shape.type = Shape.BOX
        shape.dimensions = box_dims
        box.shapes.append(shape)
        box.poses.append(box_pose)
        box.id = collision_name
        self.object_in_map_pub.publish(box)
 def remove_collision_object(self, object_id):
     '''
     Removes a collision object from the map.
     
     **Args:**
     
         **object_id (string):** The ID of the object to remove
     '''
     reset_object = CollisionObject()
     reset_object.operation.operation = reset_object.operation.REMOVE
     reset_object.header.frame_id = 'base_link'
     reset_object.header.stamp = rospy.Time.now()
     reset_object.id = object_id
     self._publish(reset_object, self._collision_object_pub)
Exemple #21
0
 def remove_collision_object(self, object_id):
     '''
     Removes a collision object from the map.
     
     **Args:**
     
         **object_id (string):** The ID of the object to remove
     '''
     reset_object = CollisionObject()
     reset_object.operation.operation = reset_object.operation.REMOVE
     reset_object.header.frame_id = 'base_link'
     reset_object.header.stamp = rospy.Time.now()
     reset_object.id = object_id
     self._publish(reset_object, self._collision_object_pub)
Exemple #22
0
def add_map_table(name, wi):
    table = rospy.get_param('wg-sushi-tables/'+name)
    upper_left = Point(x = table['upper_left']['x'], y = table['upper_left']['y'], 
                       z = table['upper_left']['z'])
    lower_left = Point(x = table['lower_left']['x'], y = table['lower_left']['y'], 
                       z = table['lower_left']['z'])
    upper_right = Point(x = table['upper_right']['x'], y = table['upper_right']['y'], 
                       z = table['upper_right']['z'])
    lower_right = Point(x = table['lower_right']['x'], y = table['lower_right']['y'], 
                       z = table['lower_right']['z'])
    table_object = CollisionObject()
    table_object.header.frame_id = wi.world_frame
    table_object.id = name
    table_pose = Pose()
    table_pose.position.x = (upper_left.x+upper_right.x+lower_left.x+lower_right.x)/4.0
    table_pose.position.y = (upper_left.y+upper_right.y+lower_left.y+lower_right.y)/4.0
    table_pose.position.z = (upper_left.z+upper_right.z+lower_left.z+lower_right.z)/4.0
    table_pose.orientation.w = 1.0
    table_shape = Shape()
    table_shape.type = Shape.MESH
    table_shape.vertices = [gt.inverse_transform_point(lower_left, table_pose), 
                                    gt.inverse_transform_point(upper_left, table_pose), 
                                    gt.inverse_transform_point(upper_right, table_pose), 
                                    gt.inverse_transform_point(lower_right, table_pose)]
    table_shape.triangles = [0, 1, 2, 2, 3, 0]
    table_object.shapes.append(table_shape)
    table_upper_pose = copy.deepcopy(table_pose)
    table_upper_pose.position.z += 0.02
    table_object.poses.append(table_pose)
    table_object.poses.append(table_upper_pose)
    table_object.shapes.append(table_shape)
    table_lower_pose = copy.deepcopy(table_pose)
    table_lower_pose.position.z -= 0.02
    table_object.poses.append(table_lower_pose)
    table_object.shapes.append(table_shape)
    if name == 'dirty_table':
        #kind of a hack =D
        for i in range(5):
            table_lower_pose = copy.deepcopy(table_lower_pose)
            table_lower_pose.position.z -= 0.02
            table_object.poses.append(table_lower_pose)
            table_object.shapes.append(table_shape)
    wi.add_object(table_object)
    table = Table()
    table.pose.header = table_object.header
    table.pose.pose = table_object.poses[0]
    table.convex_hull = table_object.shapes[0]
    return table
Exemple #23
0
 def __build_object_from_box(self, box_msg, object_id):
     msg = CollisionObject()
     
     msg.header.frame_id = box_msg.pose.header.frame_id
     msg.header.stamp = rospy.Time.now()
     msg.id = object_id
     msg.operation.operation = msg.operation.ADD
     
     shape = Shape()
     shape.type = shape.BOX
     scale = 1.0
     shape.dimensions = [scale*box_msg.box_dims.x,
                         scale*box_msg.box_dims.y,
                         scale*box_msg.box_dims.z]
     msg.shapes.append(shape)
     msg.poses.append(box_msg.pose.pose)
     return msg
Exemple #24
0
    def add_barrier(self):
        box = CollisionObject()
        box.header.frame_id = self.wi.world_frame
        box.operation.operation = box.operation.ADD
        box.id = "barrier1"

        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [0.1, 2, 1]
        box.shapes.append(shape)
        pose = Pose()
        pose.position.x = 1
        pose.position.y = -1.5
        pose.position.z = 0
        pose.orientation.w = 1.0
        box.poses.append(pose)
        self.wi.add_object(box)
Exemple #25
0
    def __build_object_from_box(self, box_msg, object_id):
        msg = CollisionObject()

        msg.header.frame_id = box_msg.pose.header.frame_id
        msg.header.stamp = rospy.Time.now()
        msg.id = object_id
        msg.operation.operation = msg.operation.ADD

        shape = Shape()
        shape.type = shape.BOX
        scale = 1.0
        shape.dimensions = [
            scale * box_msg.box_dims.x, scale * box_msg.box_dims.y,
            scale * box_msg.box_dims.z
        ]
        msg.shapes.append(shape)
        msg.poses.append(box_msg.pose.pose)
        return msg
Exemple #26
0
    def add_plate(self):
        plate = CollisionObject()
        plate.header.frame_id = self.wi.world_frame
        plate.operation.operation = plate.operation.ADD
        plate.id = "plate"

        shape = Shape()
        shape.type = shape.CYLINDER
        shape.dimensions = [0.15, 0.04]
        if self.box_plate:
            shape.type = shape.BOX
            shape.dimensions = [0.3, 0.3, 0.04]

        pose = Pose()
        if self.world < 6 or self.world == 8 or self.world == 9 or self.world == 12:
            pose.position.x = 3.1
            pose.position.y = -3.2
            pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[
                1] / 2.0
        elif self.world == 6:
            pose.position.x = 0.6  #0.894692#0.5
            pose.position.y = -0.3  #-0.468198#-0.2
            pose.position.z = CENTER_TABLE_HEIGHT + shape.dimensions[1] / 2.0
        # elif self.world == 12:
        #     pose.position.x = -1.3
        #     pose.position.y = 2
        #     pose.position.z = DOOR_TABLE_HEIGHT + 0.02 + shape.dimensions[1]/2.0
        else:
            pose.position.x = 0.5  #0.894692#0.5
            pose.position.y = 0.1
            pose.position.z = CENTER_TABLE_HEIGHT + 0.02 + shape.dimensions[
                1] / 2.0

        pose.orientation.w = 1.0

        plate.shapes.append(shape)
        plate.poses.append(pose)

        self.wi.add_object(plate)

        return plate
Exemple #27
0
 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)
Exemple #28
0
    def add_box(self):
        box = CollisionObject()
        box.header.frame_id = self.wi.world_frame
        box.operation.operation = box.operation.ADD
        box.id = "box"

        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [0.1, 0.05, 0.15]

        pose = Pose()
        pose.position.x = 3.1
        pose.position.y = -3.2
        pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
        pose.orientation.w = 1.0

        box.shapes.append(shape)
        box.poses.append(pose)

        self.wi.add_object(box)

        return box
def get_virtual_table(height = 0.42):
    table_msg = CollisionObject()

    table_msg.id = "table"
    table_msg.operation.operation = CollisionObjectOperation.ADD    

    table_msg.header.stamp = rospy.get_rostime()
    table_msg.header.frame_id = "base_footprint"
    
    side_box = Shape()
    side_box.type = Shape.BOX
    side_box.dimensions = [ 3.0, 1.0, height ]

    front_box = Shape()
    front_box.type = Shape.BOX
    front_box.dimensions = [ 1.0, 3.0, height ]

    pose = Pose()
    pose.position.x = 0.0
    pose.position.y = 0.0
    pose.position.z = height / 2
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 0
    pose.orientation.w = 1

    l_side_pose = copy.deepcopy(pose)
    l_side_pose.position.y = 0.85

    r_side_pose = copy.deepcopy(pose)
    r_side_pose.position.y = -0.85

    front_pose = copy.deepcopy(pose)
    front_pose.position.x = 0.85

    table_msg.shapes = [ side_box, side_box, front_box ]
    table_msg.poses = [ l_side_pose, r_side_pose, front_pose ]

    return table_msg
    def setUp(self):

        rospy.init_node('test_motion_execution_buffer')

        self.tf = TransformListener()

        self.obj_pub = rospy.Publisher('collision_object',
                                       CollisionObject,
                                       latch=True)

        self.move_arm_action_client = actionlib.SimpleActionClient(
            "move_right_arm", MoveArmAction)

        self.move_arm_action_client.wait_for_server()

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "obj1"
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(1)]
        obj1.shapes[0].type = Shape.CYLINDER
        obj1.shapes[0].dimensions = [float() for _ in range(2)]
        obj1.shapes[0].dimensions[0] = .1
        obj1.shapes[0].dimensions[1] = 1.5
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = .6
        obj1.poses[0].position.y = -.6
        obj1.poses[0].position.z = .75
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        self.obj_pub.publish(obj1)

        rospy.sleep(2.0)
    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
Exemple #32
0
    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 reset_collision_map(self):

        #remove all objects
        reset_object = CollisionObject()
        reset_object.operation.operation = CollisionObjectOperation.REMOVE
        reset_object.header.frame_id = "base_link"
        reset_object.header.stamp = rospy.Time.now()
        reset_object.id = "all"
        self.object_in_map_pub.publish(reset_object)

        #and all attached objects
        reset_attached_objects = AttachedCollisionObject()
        reset_attached_objects.link_name = "all"
        reset_attached_objects.object.header.frame_id = "base_link"
        reset_attached_objects.object.header.stamp = rospy.Time.now()
        reset_attached_objects.object = reset_object
        self.attached_object_pub.publish(reset_attached_objects)

        #and clear the octomap
        self.clear_octomap()

        rospy.loginfo("collision objects reset")
        self.object_in_map_current_id = 0.
        return 1
Exemple #34
0
    def add_block(self):
        block = CollisionObject()
        block.header.frame_id = self.wi.world_frame
        block.operation.operation = block.operation.ADD
        block.id = "block"
        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [0.1, 0.1, 0.13]

        pose = Pose()
        pose.orientation.w = 1.0

        block.shapes.append(shape)
        block.poses.append(pose)

        block_p = copy.deepcopy(block)
        block_p.poses[0].position.x = 2.9
        block_p.poses[0].position.y = -3.5
        block_p.poses[
            0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
        self.wi.add_object(block_p)
        return ObjectType(type="FixedObject",
                          collision_object=block,
                          parameters=['far_table'])
    def reset_collision_map(self):

        #remove all objects
        reset_object = CollisionObject()
        reset_object.operation.operation = CollisionObjectOperation.REMOVE
        reset_object.header.frame_id = "base_link"
        reset_object.header.stamp = rospy.Time.now()
        reset_object.id = "all"
        self.object_in_map_pub.publish(reset_object)

        #and all attached objects
        reset_attached_objects = AttachedCollisionObject()
        reset_attached_objects.link_name = "all"
        reset_attached_objects.object.header.frame_id = "base_link"
        reset_attached_objects.object.header.stamp = rospy.Time.now()
        reset_attached_objects.object = reset_object
        self.attached_object_pub.publish(reset_attached_objects)

        #and clear the octomap
        self.clear_octomap()

        rospy.loginfo("collision objects reset")
        self.object_in_map_current_id = 0.
        return 1
Exemple #36
0
    def add_little_object(self):
        little_obj = CollisionObject()
        little_obj.header.frame_id = self.wi.world_frame
        little_obj.operation.operation = little_obj.operation.ADD
        little_obj.id = "little_obj"
        shape = Shape()
        shape.type = shape.CYLINDER
        shape.dimensions = [0.05, 0.02]

        pose = Pose()
        pose.orientation.w = 1.0

        little_obj.shapes.append(shape)
        little_obj.poses.append(pose)

        little_obj_p = copy.deepcopy(little_obj)
        little_obj_p.poses[0].position.x = 3.1
        little_obj_p.poses[0].position.y = -3.2
        little_obj_p.poses[
            0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[1] / 2.0
        self.wi.add_object(little_obj_p)
        return ObjectType(type="RoundObject",
                          collision_object=little_obj,
                          parameters=['far_corner'])
    def setUp(self):

        self.tf = TransformListener()

        self.move_arm_action_client = actionlib.SimpleActionClient(
            "move_right_arm", MoveArmAction)

        att_pub = rospy.Publisher('attached_collision_object',
                                  AttachedCollisionObject)
        obj_pub = rospy.Publisher('collision_object', CollisionObject)

        rospy.init_node('test_motion_execution_buffer')

        #let everything settle down
        rospy.sleep(5.)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
        obj1.header.frame_id = "base_footprint"
        obj1.id = "obj2"
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(3)]
        obj1.poses = [Pose() for _ in range(3)]

        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = .5
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .2
        obj1.poses[0].position.x = .95
        obj1.poses[0].position.y = -.25
        obj1.poses[0].position.z = .62
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .5
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = 1.0
        obj1.poses[2].position.x = .95
        obj1.poses[2].position.y = -.14
        obj1.poses[2].position.z = 1.2
        obj1.poses[2].orientation.x = 0
        obj1.poses[2].orientation.y = 0
        obj1.poses[2].orientation.z = 0
        obj1.poses[2].orientation.w = 1

        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = .5
        obj1.shapes[1].dimensions[1] = .1
        obj1.shapes[1].dimensions[2] = 1.0
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = .12
        obj1.poses[1].position.z = 1.2
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1

        obj_pub.publish(obj1)

        att_object = AttachedCollisionObject()
        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.link_name = "r_gripper_r_finger_tip_link"
        att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        att_object.object = CollisionObject()

        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.object.id = "pole"
        att_object.object.shapes = [Shape() for _ in range(1)]
        att_object.object.shapes[0].type = Shape.CYLINDER
        att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
        att_object.object.shapes[0].dimensions[0] = .02
        att_object.object.shapes[0].dimensions[1] = .1
        att_object.object.poses = [Pose() for _ in range(1)]
        att_object.object.poses[0].position.x = -.02
        att_object.object.poses[0].position.y = .04
        att_object.object.poses[0].position.z = 0
        att_object.object.poses[0].orientation.x = 0
        att_object.object.poses[0].orientation.y = 0
        att_object.object.poses[0].orientation.z = 0
        att_object.object.poses[0].orientation.w = 1

        att_pub.publish(att_object)

        rospy.sleep(5.0)
Exemple #38
0
    def add_tables(self):
        if self.screenshot:
            return ['', '', '']
        table = CollisionObject()
        table.header.frame_id = self.wi.world_frame
        table.operation.operation = table.operation.ADD

        shape = Shape()
        shape.type = shape.MESH
        #center table
        shape.vertices = [
            Point(x=-0.2, y=-0.4, z=CENTER_TABLE_HEIGHT),
            Point(x=0.97, y=-0.6, z=CENTER_TABLE_HEIGHT),
            Point(x=1.0, y=0.25, z=CENTER_TABLE_HEIGHT),
            Point(x=-0.25, y=0.3, z=CENTER_TABLE_HEIGHT)
        ]
        shape.triangles = [0, 1, 2, 2, 3, 0]

        pose = Pose()
        pose.orientation.w = 1.0
        poseb = copy.deepcopy(pose)
        poseb.position.z = -0.02
        poset = copy.deepcopy(pose)
        poset.position.z = 0.02
        table.shapes.append(shape)
        table.shapes.append(shape)
        table.shapes.append(shape)
        table.poses.append(poset)
        table.poses.append(pose)
        table.poses.append(poseb)
        table.id = 'center_table'
        self.wi.add_object(table)

        #table near the door
        table.id = 'door_table'
        shape.vertices = [
            Point(x=-2.4, y=1.84, z=DOOR_TABLE_HEIGHT),
            Point(x=-1.15, y=1.75, z=DOOR_TABLE_HEIGHT),
            Point(x=-1.15, y=2.5, z=DOOR_TABLE_HEIGHT),
            Point(x=-2.4, y=2.5, z=DOOR_TABLE_HEIGHT)
        ]

        self.wi.add_object(table)

        #table in far corner
        table.id = 'far_corner'
        shape.vertices = [
            Point(x=3, y=-2.7, z=FAR_TABLE_HEIGHT),
            Point(x=2.4, y=-3.8, z=FAR_TABLE_HEIGHT),
            Point(x=3.2, y=-4.3, z=FAR_TABLE_HEIGHT),
            Point(x=3.8, y=-3.2, z=FAR_TABLE_HEIGHT)
        ]
        self.wi.add_object(table)

        if self.fake_walls:
            #these are the table feet
            foot = CollisionObject()
            foot.header.frame_id = self.wi.world_frame
            foot.operation.operation = foot.operation.ADD
            foot.id = "far_corner_foot"

            shape = Shape()
            shape.type = shape.BOX
            shape.dimensions = [0.1, 0.5, FAR_TABLE_HEIGHT / 2.0]

            pose = Pose()
            pose.position.x = 3
            pose.position.y = -3.4
            pose.position.z = shape.dimensions[2] / 2.0
            angle = 0.5
            pose.orientation.z = np.cos(angle / 2.0)
            pose.orientation.w = np.sin(angle / 2.0)
            foot.shapes.append(shape)
            foot.poses.append(pose)
            self.wi.add_object(foot)

            foot.id = "center_table_foot1"
            shape.dimensions = [0.1, 0.75, 0.3]
            pose.position.x = 0.9
            pose.position.y = -0.1
            pose.position.z = shape.dimensions[2] / 2.0
            angle = 0
            pose.orientation.z = np.cos(angle / 2.0)
            pose.orientation.w = np.sin(angle / 2.0)
            self.wi.add_object(foot)

            foot.id = "center_table_foot2"
            pose.position.x = -0.2
            self.wi.add_object(foot)

            foot.id = "door_table_foot"
            pose.position.x = -1.25
            pose.position.y = 2.1
            self.wi.add_object(foot)

        return ['center_table', 'door_table', 'far_corner']
    def testAllowedNotAllowedInitialContact(self):

        #adding object in collision with base

        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "base_object"
        obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj2.shapes = [Shape() for _ in range(1)]
        obj2.shapes[0].type = Shape.BOX
        obj2.shapes[0].dimensions = [float() for _ in range(3)]
        obj2.shapes[0].dimensions[0] = .1
        obj2.shapes[0].dimensions[1] = .1
        obj2.shapes[0].dimensions[2] = .1
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = 0
        obj2.poses[0].position.y = 0
        obj2.poses[0].position.z = 0
        obj2.poses[0].orientation.x = 0
        obj2.poses[0].orientation.y = 0
        obj2.poses[0].orientation.z = 0
        obj2.poses[0].orientation.w = 1

        self.obj_pub.publish(obj2)

        rospy.sleep(5.)

        joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
        goal = MoveArmGoal()

        goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]

        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 1
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"

        goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
        for i in range(len(joint_names)):
            goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
            goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
            goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
            goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08

        goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
        goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
               cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)

        # but we can still overwrite
        coll = CollisionOperation()
        coll.object1 = "base_link"
        coll.object2 = coll.COLLISION_SET_OBJECTS
        coll.operation = coll.ENABLE

        goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll)

        goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
        goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
               cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
    def testAllowedNotAllowedInitialContact(self):

        #adding object in collision with base

        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "base_object"
        obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj2.shapes = [Shape() for _ in range(1)]
        obj2.shapes[0].type = Shape.BOX
        obj2.shapes[0].dimensions = [float() for _ in range(3)]
        obj2.shapes[0].dimensions[0] = .1
        obj2.shapes[0].dimensions[1] = .1
        obj2.shapes[0].dimensions[2] = .1
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = 0
        obj2.poses[0].position.y = 0
        obj2.poses[0].position.z = 0
        obj2.poses[0].orientation.x = 0
        obj2.poses[0].orientation.y = 0
        obj2.poses[0].orientation.z = 0
        obj2.poses[0].orientation.w = 1

        self.obj_pub.publish(obj2)

        rospy.sleep(5.)

        joint_names = [
            '%s_%s' % ('r', j) for j in [
                'shoulder_pan_joint', 'shoulder_lift_joint',
                'upper_arm_roll_joint', 'elbow_flex_joint',
                'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
            ]
        ]
        goal = MoveArmGoal()

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]

        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 1
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]
        for i in range(len(joint_names)):
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].joint_name = joint_names[i]
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].position = 0.0
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_above = 0.08
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_below = 0.08

        goal.motion_plan_request.goal_constraints.joint_constraints[
            0].position = -2.0
        goal.motion_plan_request.goal_constraints.joint_constraints[
            3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[
            5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)

        # but we can still overwrite
        coll = CollisionOperation()
        coll.object1 = "base_link"
        coll.object2 = coll.COLLISION_SET_OBJECTS
        coll.operation = coll.ENABLE

        goal.motion_plan_request.ordered_collision_operations.collision_operations.append(
            coll)

        goal.motion_plan_request.goal_constraints.joint_constraints[
            0].position = 0.0
        goal.motion_plan_request.goal_constraints.joint_constraints[
            3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[
            5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
Exemple #41
0
def test_add_convert_objects():

    obj_pub = rospy.Publisher('collision_object',CollisionObject)
    att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)

    rospy.init_node('test_collision_objects')

    #let everything settle down
    rospy.sleep(5.)
      
    obj1 = CollisionObject()
    
    obj1.header.stamp = rospy.Time.now()
    obj1.header.frame_id = "base_link"
    obj1.id = "obj1";
    obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
    obj1.shapes = [Shape() for _ in range(1)]
    obj1.shapes[0].type = Shape.BOX
    obj1.shapes[0].dimensions = [float() for _ in range(3)]
    obj1.shapes[0].dimensions[0] = .1
    obj1.shapes[0].dimensions[1] = .1
    obj1.shapes[0].dimensions[2] = .75
    obj1.poses = [Pose() for _ in range(1)]
    obj1.poses[0].position.x = .6
    obj1.poses[0].position.y = -.6
    obj1.poses[0].position.z = .375
    obj1.poses[0].orientation.x = 0
    obj1.poses[0].orientation.y = 0
    obj1.poses[0].orientation.z = 0
    obj1.poses[0].orientation.w = 1

    att_obj = AttachedCollisionObject()
    att_obj.link_name = "r_gripper_palm_link"
    att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
                           'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
    obj2 = CollisionObject()
    
    obj2.header.stamp = rospy.Time.now()
    obj2.header.frame_id = "r_gripper_palm_link"
    obj2.id = "obj2";
    obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
    obj2.shapes = [Shape() for _ in range(1)]
    obj2.shapes[0].type = Shape.CYLINDER
    obj2.shapes[0].dimensions = [float() for _ in range(2)]
    obj2.shapes[0].dimensions[0] = .025
    obj2.shapes[0].dimensions[1] = .5
    obj2.poses = [Pose() for _ in range(1)]
    obj2.poses[0].position.x = .12
    obj2.poses[0].position.y = 0
    obj2.poses[0].position.z = 0
    obj2.poses[0].orientation.x = 0
    obj2.poses[0].orientation.y = 0
    obj2.poses[0].orientation.z = 0
    obj2.poses[0].orientation.w = 1
    att_obj.object = obj2
    r = rospy.Rate(.1)

    while(True):
        obj1.header.stamp = rospy.Time.now()
        obj_pub.publish(obj1)
        att_obj.object.header.stamp = rospy.Time.now()
        att_pub.publish(att_obj)
        r.sleep()
Exemple #42
0
def get_obj():

    attached_obj = AttachedCollisionObject()
    obj = CollisionObject()
    shape = Shape()
    pose = Pose()
    vert = Point()
    verts = []

    in_verts = True
    in_position = True

    f = open('test.txt', 'r')
    for line in f:
        fields = line.split(':')
        fields = [fields[i].strip() for i in range(len(fields))]
        #        print fields
        if fields[0] == "frame_id":
            obj.header.frame_id = fields[1]
        elif fields[0] == "id":
            obj.id = fields[1]
        elif fields[0] == "operation" and fields[1] != "":
            obj.operation.operation = int(fields[1])
        elif fields[0] == "type":
            shape.type = int(fields[1])
        elif fields[0] == "triangles":
            array = fields[1][1:-2]
            ind = array.split(',')
            inds = [int(i) for i in ind]
            shape.triangles = inds
        elif fields[0] == "x" and in_verts:
            vert = Point()
            vert.x = float(fields[1])
        elif fields[0] == "y" and in_verts:
            vert.y = float(fields[1])
        elif fields[0] == "z" and in_verts:
            vert.z = float(fields[1])
            verts.append(vert)
        elif fields[0] == "poses":
            in_verts = False
        elif fields[0] == "x" and in_position:
            pose.position.x = float(fields[1])
        elif fields[0] == "y" and in_position:
            pose.position.y = float(fields[1])
        elif fields[0] == "z" and in_position:
            pose.position.z = float(fields[1])
            in_position = False
        elif fields[0] == "x" and not in_position:
            pose.orientation.x = float(fields[1])
        elif fields[0] == "y" and not in_position:
            pose.orientation.y = float(fields[1])
        elif fields[0] == "z" and not in_position:
            pose.orientation.z = float(fields[1])
        elif fields[0] == "w":
            pose.orientation.w = float(fields[1])

        obj.id = "graspable_object_1001"
        shape.vertices = verts
        obj.shapes = [shape]
        obj.poses = [pose]

    attached_obj.object = obj
    return attached_obj
    def setUp(self):

        self.tf = TransformListener()
        
        self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)



        att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
        obj_pub = rospy.Publisher('collision_object',CollisionObject)
        
        rospy.init_node('test_motion_execution_buffer')
        
        #let everything settle down
        rospy.sleep(5.)
        
        obj1 = CollisionObject()
    
        obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
        obj1.header.frame_id = "base_footprint"
        obj1.id = "obj2";
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(3)]
        obj1.poses = [Pose() for _ in range(3)]

        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = .5
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .2
        obj1.poses[0].position.x = .95
        obj1.poses[0].position.y = -.25
        obj1.poses[0].position.z = .62
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .5
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = 1.0
        obj1.poses[2].position.x = .95
        obj1.poses[2].position.y = -.14
        obj1.poses[2].position.z = 1.2
        obj1.poses[2].orientation.x = 0
        obj1.poses[2].orientation.y = 0
        obj1.poses[2].orientation.z = 0
        obj1.poses[2].orientation.w = 1

        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = .5
        obj1.shapes[1].dimensions[1] = .1
        obj1.shapes[1].dimensions[2] = 1.0
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = .12
        obj1.poses[1].position.z = 1.2
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1
        
        obj_pub.publish(obj1)

        att_object = AttachedCollisionObject()
        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.link_name = "r_gripper_r_finger_tip_link"
        att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        att_object.object = CollisionObject();

        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.object.id = "pole"
        att_object.object.shapes = [Shape() for _ in range(1)]
        att_object.object.shapes[0].type = Shape.CYLINDER
        att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
        att_object.object.shapes[0].dimensions[0] = .02
        att_object.object.shapes[0].dimensions[1] = .1
        att_object.object.poses = [Pose() for _ in range(1)]
        att_object.object.poses[0].position.x = -.02
        att_object.object.poses[0].position.y = .04
        att_object.object.poses[0].position.z = 0
        att_object.object.poses[0].orientation.x = 0
        att_object.object.poses[0].orientation.y = 0
        att_object.object.poses[0].orientation.z = 0
        att_object.object.poses[0].orientation.w = 1

        att_pub.publish(att_object)

        rospy.sleep(5.0)        
    def setUp(self):

        self.tf = TransformListener()
        
        self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)

        obj_pub = rospy.Publisher('collision_object',CollisionObject)
        
        rospy.init_node('test_motion_execution_buffer')
        
        #let everything settle down
        rospy.sleep(5.)
        
        obj1 = CollisionObject()
    
        obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
        obj1.header.frame_id = "base_footprint"
        obj1.id = "obj2";
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(4)]
        obj1.poses = [Pose() for _ in range(4)]

        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = .5
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .2
        obj1.poses[0].position.x = .95
        obj1.poses[0].position.y = -.25
        obj1.poses[0].position.z = .62
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = .5
        obj1.shapes[1].dimensions[1] = 1.0
        obj1.shapes[1].dimensions[2] = .2
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = -.25
        obj1.poses[1].position.z = .92
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .2
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = .2
        obj1.poses[2].position.x = .8
        obj1.poses[2].position.y = -.5
        obj1.poses[2].position.z = .78
        obj1.poses[2].orientation.x = 0
        obj1.poses[2].orientation.y = 0
        obj1.poses[2].orientation.z = 0
        obj1.poses[2].orientation.w = 1

        obj1.shapes[3].type = Shape.BOX
        obj1.shapes[3].dimensions = [float() for _ in range(3)]
        obj1.shapes[3].dimensions[0] = .2
        obj1.shapes[3].dimensions[1] = .1
        obj1.shapes[3].dimensions[2] = .2
        obj1.poses[3].position.x = .8
        obj1.poses[3].position.y = .18
        obj1.poses[3].position.z = .78
        obj1.poses[3].orientation.x = 0
        obj1.poses[3].orientation.y = 0
        obj1.poses[3].orientation.z = 0
        obj1.poses[3].orientation.w = 1
        obj_pub.publish(obj1)

        rospy.sleep(5.0)
Exemple #45
0
'''This file has hardcoded publishing for the table collision positions in USARSim, to help out the arm navigation planner.
  In the future, it will be far more reasonable to publish this information by reading it from the database.'''

import roslib
roslib.load_manifest('usarsim_tools')
import rospy


from arm_navigation_msgs.msg import CollisionObjectOperation, CollisionObject, Shape
from geometry_msgs.msg import Pose
from visualization_msgs.msg import Marker
import copy

rospy.init_node("collision_add")

table1 = CollisionObject()
table2 = CollisionObject()
cShape = Shape()
cShape.type = Shape.BOX
cShape.dimensions = (3.968, 2.880, 1.088)

table1.poses.append(Pose())
table1.poses[0].position.x = 1.468
table1.poses[0].position.y = 1.108
table1.poses[0].position.z = .8
#cPose.position.x = -4.416
#cPose.position.y = 0.992
#cPose.position.z = .8
table1.poses[0].orientation.x = 0.0
table1.poses[0].orientation.y = 0.0
table1.poses[0].orientation.z = 0.0
Exemple #46
0
    def setUp(self):

        self.tf = TransformListener()

        self.move_arm_action_client = actionlib.SimpleActionClient(
            "move_right_arm", MoveArmAction)

        obj_pub = rospy.Publisher('collision_object', CollisionObject)

        rospy.init_node('test_motion_execution_buffer')

        #let everything settle down
        rospy.sleep(5.)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
        obj1.header.frame_id = "base_footprint"
        obj1.id = "obj2"
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(4)]
        obj1.poses = [Pose() for _ in range(4)]

        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = .5
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .2
        obj1.poses[0].position.x = .95
        obj1.poses[0].position.y = -.25
        obj1.poses[0].position.z = .62
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = .5
        obj1.shapes[1].dimensions[1] = 1.0
        obj1.shapes[1].dimensions[2] = .2
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = -.25
        obj1.poses[1].position.z = .92
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .2
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = .2
        obj1.poses[2].position.x = .8
        obj1.poses[2].position.y = -.5
        obj1.poses[2].position.z = .78
        obj1.poses[2].orientation.x = 0
        obj1.poses[2].orientation.y = 0
        obj1.poses[2].orientation.z = 0
        obj1.poses[2].orientation.w = 1

        obj1.shapes[3].type = Shape.BOX
        obj1.shapes[3].dimensions = [float() for _ in range(3)]
        obj1.shapes[3].dimensions[0] = .2
        obj1.shapes[3].dimensions[1] = .1
        obj1.shapes[3].dimensions[2] = .2
        obj1.poses[3].position.x = .8
        obj1.poses[3].position.y = .18
        obj1.poses[3].position.z = .78
        obj1.poses[3].orientation.x = 0
        obj1.poses[3].orientation.y = 0
        obj1.poses[3].orientation.z = 0
        obj1.poses[3].orientation.w = 1
        obj_pub.publish(obj1)

        rospy.sleep(5.0)
Exemple #47
0
    def add_spatula(self, arm):
        spatula = CollisionObject()
        spatula.id = "spatula"
        spatula.header.frame_id = self.wi.world_frame
        spatula.operation.operation = spatula.operation.ADD

        paddle = Shape()
        handle = Shape()
        paddle.type = paddle.BOX
        paddle.dimensions = [0.11, 0.12, 0.005]
        handle.type = handle.CYLINDER
        handle.dimensions = [0.02, 0.24]

        paddle_pose = Pose()
        handle_pose = Pose()
        paddle_pose.position.y = paddle.dimensions[1] / 2.0
        paddle_pose.orientation.w = 1.0

        angle = np.pi / 5.0
        handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
            np.pi / 2.0 - angle)
        handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
            np.pi / 2.0 - angle)
        handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
        handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)

        spatula.shapes = [paddle, handle]
        spatula.poses = [paddle_pose, handle_pose]

        #this is the grasp transformation
        pos_on_handle = handle.dimensions[1] - 0.1
        inv_grasp = Transform()
        grasp = RigidGrasp()
        #really should be calculating this...
        inv_grasp.translation.y = GRIPPER_LENGTH
        inv_grasp.translation.z = pos_on_handle / 2.0
        #flip 90 degrees
        inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
        inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
        g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
        origin = Pose()
        origin.orientation.w = 1.0
        grasp.transform = pose_to_transform(
            gt.inverse_transform_pose(origin, g))
        grasp.touch_links = [arm[0] + '_end_effector']
        grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
        grasp.min_approach_distance = 0
        grasp.desired_approach_distance = 0.15
        grasp.min_distance_from_surface = -1

        spat_p = copy.deepcopy(spatula)

        wtrans = Pose()
        wtrans.orientation.x = np.sin(angle / 2.0)
        wtrans.orientation.w = np.cos(angle / 2.0)
        if self.world == -1:
            wtrans.position.x = 3
            wtrans.position.y = -2.8
            wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['far_corner']
        elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
            wtrans.position.x = -1.7
            wtrans.position.y = 2
            wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['door_table']
        else:
            wtrans.position.x = 0.6
            wtrans.position.y = -0.3
            wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
                0]
            ss = ['center_table']
            if self.world == -4 or self.world == -5:
                wtrans.position.y = 0
                rot = Quaternion()
                rot.z = np.sin(np.pi / 2.0)
                rot.w = np.cos(np.pi / 2.0)
                wtrans.orientation = gt.multiply_quaternions(
                    rot, wtrans.orientation)
                if self.world == -5:
                    wtrans.position.x = 0

        for i in range(len(spat_p.poses)):
            spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)

        self.wi.add_object(spat_p)
        return ObjectType(type="SpatulaObject",
                          collision_object=spatula,
                          parameters=ss,
                          numeric_parameters=paddle.dimensions +
                          handle.dimensions + [angle]), [grasp]
    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)
Exemple #49
0
    def add_walls(self):
        window_wall = CollisionObject()
        window_wall.header.frame_id = self.wi.world_frame
        window_wall.operation.operation = window_wall.operation.ADD
        window_wall.id = "window_wall"

        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [8, 0.1, 2.5]
        pose = Pose()
        pose.position.x = 0
        pose.position.y = -2.63
        pose.position.z = shape.dimensions[2] / 2.0 - 0.1
        angle = np.pi / 6.25
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.shapes.append(shape)
        window_wall.poses.append(pose)
        self.wi.add_object(window_wall)

        shape.dimensions[0] = shape.dimensions[1]
        shape.dimensions[1] = 5
        pose.position.x = -2.45
        pose.position.y = 1
        angle = 0.05
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "door_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[1] = shape.dimensions[0]
        shape.dimensions[0] = 8
        pose.position.x = 0
        pose.position.y = 2.3
        angle = 0.1
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "cabinet_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[0] = shape.dimensions[1]
        shape.dimensions[1] = 3
        pose.position.x = 4
        pose.position.y = -3
        angle = 0.5
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "banner_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[1] = 4
        shape.dimensions[2] = 1
        pose.position.x = 3.25
        pose.position.y = 0.65
        pose.position.z = shape.dimensions[2] / 2.0 - 0.05
        angle = 0
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "computer_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[1] = shape.dimensions[0]
        shape.dimensions[0] = 1.8
        pose.position.x = 3.9
        pose.position.y = -1.6
        angle = np.pi / 5.5
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "nook_wall"
        self.wi.add_object(window_wall)