def create_drawable_sphere_object(): """Create a drawable sphere world object that can be added through a mutation request.""" # Add an edge where the transform vision_tform_object is only a position transform with no rotation. vision_tform_drawable = geom.SE3Pose(position=geom.Vec3(x=2, y=3, z=2), rotation=geom.Quaternion(x=0, y=0, z=0, w=1)) # Create a map between the child frame name and the parent frame name/SE3Pose parent_tform_child edges = {} # Create an edge in the frame tree snapshot that includes vision_tform_drawable drawable_frame_name = "drawing_sphere" edges = add_edge_to_tree(edges, vision_tform_drawable, VISION_FRAME_NAME, drawable_frame_name) snapshot = geom.FrameTreeSnapshot(child_to_parent_edge_map=edges) # Set the acquisition time for the sphere using a function to get google.protobuf.Timestamp of the current system time. time_now = now_timestamp() # Create the sphere drawable object sphere = world_object_pb2.DrawableSphere(radius=1) red_color = world_object_pb2.DrawableProperties.Color(r=255, b=0, g=0, a=1) sphere_drawable_prop = world_object_pb2.DrawableProperties( color=red_color, label="red sphere", wireframe=False, sphere=sphere, frame_name_drawable=drawable_frame_name) # Create the complete world object with transform information, a unique name, and the drawable sphere properties. world_object_sphere = world_object_pb2.WorldObject(id=16, name="red_sphere_ball", transforms_snapshot=snapshot, acquisition_time=time_now, drawable_properties=[sphere_drawable_prop]) return world_object_sphere
def create_apriltag_object(): """Create an apriltag world object that can be added through a mutation request.""" # Set the acquisition time for the additional april tag in robot time using a function to # get google.protobuf.Timestamp of the current system time. time_now = now_timestamp() # The apriltag id for the object we want to add. tag_id = 308 # Set the frame names used for the two variants of the apriltag (filtered, raw) frame_name_fiducial = "fiducial_" + str(tag_id) frame_name_fiducial_filtered = "filtered_fiducial_" + str(tag_id) # Make the april tag (slightly offset from the first tag detection) as a world object. Create the # different edges necessary to create an expressive tree. The root node will be the world frame. default_a_tform_b = geom.SE3Pose(position=geom.Vec3(x=.2, y=.2, z=.2), rotation=geom.Quaternion(x=.1, y=.1, z=.1, w=.1)) # Create a map between the child frame name and the parent frame name/SE3Pose parent_tform_child edges = {} # Create an edge for the raw fiducial detection in the world. vision_tform_fiducial = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_fiducial, VISION_FRAME_NAME, frame_name_fiducial) # Create a edge for the filtered version of the fiducial in the world. vision_tform_filtered_fiducial = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_filtered_fiducial, VISION_FRAME_NAME, frame_name_fiducial_filtered) # Create the transform to express vision_tform_odom vision_tform_odom = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_odom, VISION_FRAME_NAME, ODOM_FRAME_NAME) # Can also add custom frames into the frame tree snapshot as long as they keep the tree structure, # so the parent_frame must also be in the tree. vision_tform_special_frame = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_special_frame, VISION_FRAME_NAME, "my_special_frame") snapshot = geom.FrameTreeSnapshot(child_to_parent_edge_map=edges) # Create the specific properties for the apriltag including the frame names for the transforms # describing the apriltag's position. tag_prop = world_object_pb2.AprilTagProperties( tag_id=tag_id, dimensions=geom.Vec2(x=.2, y=.2), frame_name_fiducial=frame_name_fiducial, frame_name_fiducial_filtered=frame_name_fiducial_filtered) #Create the complete world object with transform information and the apriltag properties. wo_obj_to_add = world_object_pb2.WorldObject(id=21, transforms_snapshot=snapshot, acquisition_time=time_now, apriltag_properties=tag_prop) return wo_obj_to_add
def _create_snapshot(frame_tree_snapshot_string): frame_tree_snapshot = geom_protos.FrameTreeSnapshot() google.protobuf.text_format.Parse(frame_tree_snapshot_string, frame_tree_snapshot) return frame_tree_snapshot