def test_frame_tree_math_single_edge(): snapshot_text = """child_to_parent_edge_map { key: "beta" value: { parent_frame_name: "alpha" parent_tform_child: { position: { x: 10 y: 0 z: 0 } } } } child_to_parent_edge_map { key: "alpha" value: { parent_frame_name: "" } }""" frame_tree = _create_snapshot(snapshot_text) assert frame_helpers.validate_frame_tree_snapshot(frame_tree) assert _do_poses_match( 10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'beta')) assert _do_poses_match( -10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'alpha')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'alpha')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'beta')) assert not frame_helpers.get_a_tform_b(frame_tree, 'omega', 'alpha') assert not frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'omega') assert not frame_helpers.get_a_tform_b(frame_tree, 'omega', 'omega') assert not frame_helpers.get_a_tform_b(frame_tree, 'omaga', 'psi')
def __init__(self, image_response): self.image = self.extract_image(image_response) self.body_T_image_sensor = get_a_tform_b(image_response.shot.transforms_snapshot, \ BODY_FRAME_NAME, image_response.shot.frame_name_image_sensor) self.vision_T_body = get_vision_tform_body(image_response.shot.transforms_snapshot) if not self.body_T_image_sensor: raise Exception("Won't work.") if image_response.source.pinhole: resolution = numpy.asarray([ \ image_response.source.cols, \ image_response.source.rows]) focal_length = numpy.asarray([ \ image_response.source.pinhole.intrinsics.focal_length.x, \ image_response.source.pinhole.intrinsics.focal_length.y]) principal_point = numpy.asarray([ \ image_response.source.pinhole.intrinsics.principal_point.x, \ image_response.source.pinhole.intrinsics.principal_point.y]) else: raise Exception("Won't work.") sensor_T_vo = (self.vision_T_body * self.body_T_image_sensor).inverse() camera_projection_mat = numpy.eye(4) camera_projection_mat[0, 0] = (focal_length[0] / resolution[0]) camera_projection_mat[0, 2] = (principal_point[0] / resolution[0]) camera_projection_mat[1, 1] = (focal_length[1] / resolution[1]) camera_projection_mat[1, 2] = (principal_point[1] / resolution[1]) self.MVP = camera_projection_mat.dot(sensor_T_vo.to_matrix())
def get_terrain_markers(local_grid_proto): '''Receives raw proto from grid_client.get_local_grids(...) and returns marker array msg''' for local_grid in local_grid_proto: if local_grid.local_grid_type_name == "terrain": #TODO: Support parsing the terrain_valid and the intensity fields in the proto vision_tform_local_grid = get_a_tform_b( local_grid.local_grid.transforms_snapshot, VISION_FRAME_NAME, local_grid.local_grid.frame_name_local_grid_data).to_proto() cell_size = local_grid.local_grid.extent.cell_size terrain_pts = get_terrain_grid(local_grid) # terrain_pts is [[x1,y1,z1], [x2,y2,z2], [x3,y3,z3], ...] (a list of lists, each of which is a point) # in the correct relative pose to Spot's body terrain_pts = offset_grid_pixels(terrain_pts, vision_tform_local_grid, cell_size) # Parse terrain_pts into Marker (cube_list) # Note: Using a single Marker of type CUBE_LIST allows for batch rendering, whereas MarkerArray of CUBEs does not marker = visualization_msgs.msg.Marker() marker.header.seq = 0 marker.id = 0 marker.header.stamp = rospy.Time() marker.header.frame_id = "vision_odometry_frame" #Must be a frame that exists (e.g. Spot's vision_odometry_frame) marker.type = visualization_msgs.msg.Marker.CUBE_LIST marker.action = visualization_msgs.msg.Marker.ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = cell_size marker.scale.y = cell_size marker.scale.z = cell_size marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 1 for terrain_pt in terrain_pts: p = geometry_msgs.msg.Point() c = std_msgs.msg.ColorRGBA() # Cube location p.x = terrain_pt[0] p.y = terrain_pt[1] p.z = terrain_pt[2] #Make color change with distance from Spot # max_dist = math.sqrt(local_grid_proto[0].local_grid.extent.num_cells_x**2 + local_grid_proto[0].local_grid.extent.num_cells_y**2)/2.0 * local_grid_proto[0].local_grid.extent.cell_size # c.r = math.sqrt(p.x**2+p.y**2)/(max_dist+0.1) # c.g = 1 - math.sqrt(p.x**2+p.y**2)/(max_dist) # c.b = math.sqrt(p.x**2+p.y**2)/(max_dist+0.1) # c.a = 1.0 marker.points.append(p) # marker.colors.append(c) return marker
def image_to_bounding_box(self): """Determine which camera source has a fiducial. Return the bounding box of the first detected fiducial.""" #Iterate through all five camera sources to check for a fiducial for i in range(len(self._source_names) + 1): # Get the image from the source camera. if i == 0: if self._previous_source != None: # Prioritize the camera the fiducial was last detected in. source_name = self._previous_source else: continue elif self._source_names[i - 1] == self._previous_source: continue else: source_name = self._source_names[i - 1] img_req = build_image_request( source_name, quality_percent=100, image_format=image_pb2.Image.FORMAT_RAW) image_response = self._image_client.get_image([img_req]) self._camera_tform_body = get_a_tform_b( image_response[0].shot.transforms_snapshot, image_response[0].shot.frame_name_image_sensor, BODY_FRAME_NAME) self._body_tform_world = get_a_tform_b( image_response[0].shot.transforms_snapshot, BODY_FRAME_NAME, VISION_FRAME_NAME) # Camera intrinsics for the given source camera. self._intrinsics = image_response[0].source.pinhole.intrinsics width = image_response[0].shot.image.cols height = image_response[0].shot.image.rows # detect given fiducial in image and return the bounding box of it bboxes = self.detect_fiducial_in_image( image_response[0].shot.image, (width, height), source_name) if bboxes: print("Found bounding box for " + str(source_name)) return bboxes, source_name else: self._tag_not_located = True print("Failed to find bounding box for " + str(source_name)) return [], None
def start(self): """Claim lease of robot and start the fiducial follower.""" self._lease = self._lease_client.acquire() self._robot.time_sync.wait_for_sync() self._lease_keepalive = bosdyn.client.lease.LeaseKeepAlive( self._lease_client) # Stand the robot up. if self._standup: self.power_on() blocking_stand(self._robot_command_client) # Delay grabbing image until spot is standing (or close enough to upright). time.sleep(.35) while self._attempts <= self._max_attempts: detected_fiducial = False fiducial_rt_world = None if self._use_world_object_service: # Get the first fiducial object Spot detects with the world object service. fiducial = self.get_fiducial_objects() if fiducial is not None: vision_tform_fiducial = get_a_tform_b( fiducial.transforms_snapshot, VISION_FRAME_NAME, fiducial.apriltag_properties.frame_name_fiducial ).to_proto() if vision_tform_fiducial is not None: detected_fiducial = True fiducial_rt_world = vision_tform_fiducial.position else: # Detect the april tag in the images from Spot using the apriltag library. bboxes, source_name = self.image_to_bounding_box() if bboxes: self._previous_source = source_name (tvec, _, source_name) = self.pixel_coords_to_camera_coords( bboxes, self._intrinsics, source_name) vision_tform_fiducial_position = self.compute_fiducial_in_world_frame( tvec) fiducial_rt_world = geometry_pb2.Vec3( x=vision_tform_fiducial_position[0], y=vision_tform_fiducial_position[1], z=vision_tform_fiducial_position[2]) detected_fiducial = True if detected_fiducial: # Go to the tag and stop within a certain distance self.go_to_tag(fiducial_rt_world) else: print("No fiducials found") self._attempts += 1 #increment attempts at finding a fiducial # Power off at the conclusion of the example. if self._powered_on: self.power_off()
def vision_tform_sensor(self): """Look up vision_tform_sensor for sensor which user clicked. Returns: math_helpers.SE3Pose """ clicked_image_proto = self.image_dict[self.clicked_source][0] frame_name_image_sensor = clicked_image_proto.shot.frame_name_image_sensor snapshot = clicked_image_proto.shot.transforms_snapshot return frame_helpers.get_a_tform_b(snapshot, frame_helpers.VISION_FRAME_NAME, frame_name_image_sensor)
def compute_stand_location_and_yaw(vision_tform_target, robot_state_client, distance_margin): # Compute drop-off location: # Draw a line from Spot to the person # Back up 2.0 meters on that line vision_tform_robot = frame_helpers.get_a_tform_b( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot, frame_helpers.VISION_FRAME_NAME, frame_helpers.GRAV_ALIGNED_BODY_FRAME_NAME) # Compute vector between robot and person robot_rt_person_ewrt_vision = [ vision_tform_robot.x - vision_tform_target.x, vision_tform_robot.y - vision_tform_target.y, vision_tform_robot.z - vision_tform_target.z ] # Compute the unit vector. if np.linalg.norm(robot_rt_person_ewrt_vision) < 0.01: robot_rt_person_ewrt_vision_hat = vision_tform_robot.transform_point( 1, 0, 0) else: robot_rt_person_ewrt_vision_hat = robot_rt_person_ewrt_vision / np.linalg.norm( robot_rt_person_ewrt_vision) # Starting at the person, back up meters along the unit vector. drop_position_rt_vision = [ vision_tform_target.x + robot_rt_person_ewrt_vision_hat[0] * distance_margin, vision_tform_target.y + robot_rt_person_ewrt_vision_hat[1] * distance_margin, vision_tform_target.z + robot_rt_person_ewrt_vision_hat[2] * distance_margin ] # We also want to compute a rotation (yaw) so that we will face the person when dropping. # We'll do this by computing a rotation matrix with X along # -robot_rt_person_ewrt_vision_hat (pointing from the robot to the person) and Z straight up: xhat = -robot_rt_person_ewrt_vision_hat zhat = [0.0, 0.0, 1.0] yhat = np.cross(zhat, xhat) mat = np.matrix([xhat, yhat, zhat]).transpose() heading_rt_vision = math_helpers.Quat.from_matrix(mat).to_yaw() return drop_position_rt_vision, heading_rt_vision
def get_obj_and_img(network_compute_client, server, model, confidence, image_sources, label): for source in image_sources: # Build a network compute request for this image source. image_source_and_service = network_compute_bridge_pb2.ImageSourceAndService( image_source=source) # Input data: # model name # minimum confidence (between 0 and 1) # if we should automatically rotate the image input_data = network_compute_bridge_pb2.NetworkComputeInputData( image_source_and_service=image_source_and_service, model_name=model, min_confidence=confidence, rotate_image=network_compute_bridge_pb2.NetworkComputeInputData. ROTATE_IMAGE_ALIGN_HORIZONTAL) # Server data: the service name server_data = network_compute_bridge_pb2.NetworkComputeServerConfiguration( service_name=server) # Pack and send the request. process_img_req = network_compute_bridge_pb2.NetworkComputeRequest( input_data=input_data, server_config=server_data) resp = network_compute_client.network_compute_bridge_command( process_img_req) best_obj = None highest_conf = 0.0 best_vision_tform_obj = None img = get_bounding_box_image(resp) image_full = resp.image_response # Show the image cv2.imshow("Fetch", img) cv2.waitKey(15) if len(resp.object_in_image) > 0: for obj in resp.object_in_image: # Get the label obj_label = obj.name.split('_label_')[-1] if obj_label != label: continue conf_msg = wrappers_pb2.FloatValue() obj.additional_properties.Unpack(conf_msg) conf = conf_msg.value try: vision_tform_obj = frame_helpers.get_a_tform_b( obj.transforms_snapshot, frame_helpers.VISION_FRAME_NAME, obj.image_properties.frame_name_image_coordinates) except bosdyn.client.frame_helpers.ValidateFrameTreeError: # No depth data available. vision_tform_obj = None if conf > highest_conf and vision_tform_obj is not None: highest_conf = conf best_obj = obj best_vision_tform_obj = vision_tform_obj if best_obj is not None: return best_obj, image_full, best_vision_tform_obj return None, None, None
def main(argv): parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) parser.add_argument( '-s', '--ml-service', help='Service name of external machine learning server.', required=True) parser.add_argument('-m', '--model', help='Model name running on the external server.', required=True) parser.add_argument( '-p', '--person-model', help='Person detection model name running on the external server.') parser.add_argument( '-c', '--confidence-dogtoy', help= 'Minimum confidence to return an object for the dogoy (0.0 to 1.0)', default=0.5, type=float) parser.add_argument( '-e', '--confidence-person', help='Minimum confidence for person detection (0.0 to 1.0)', default=0.6, type=float) options = parser.parse_args(argv) cv2.namedWindow("Fetch") cv2.waitKey(500) sdk = bosdyn.client.create_standard_sdk('SpotFetchClient') sdk.register_service_client(NetworkComputeBridgeClient) robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) # Time sync is necessary so that time-based filter requests can be converted robot.time_sync.wait_for_sync() network_compute_client = robot.ensure_client( NetworkComputeBridgeClient.default_service_name) robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) command_client = robot.ensure_client( RobotCommandClient.default_service_name) lease_client = robot.ensure_client(LeaseClient.default_service_name) manipulation_api_client = robot.ensure_client( ManipulationApiClient.default_service_name) # This script assumes the robot is already standing via the tablet. We'll take over from the # tablet. lease = lease_client.take() lk = bosdyn.client.lease.LeaseKeepAlive(lease_client) # Store the position of the hand at the last toy drop point. vision_tform_hand_at_drop = None while True: holding_toy = False while not holding_toy: # Capture an image and run ML on it. dogtoy, image, vision_tform_dogtoy = get_obj_and_img( network_compute_client, options.ml_service, options.model, options.confidence_dogtoy, kImageSources, 'dogtoy') if dogtoy is None: # Didn't find anything, keep searching. continue # If we have already dropped the toy off, make sure it has moved a sufficient amount before # picking it up again if vision_tform_hand_at_drop is not None and pose_dist( vision_tform_hand_at_drop, vision_tform_dogtoy) < 0.5: print('Found dogtoy, but it hasn\'t moved. Waiting...') time.sleep(1) continue print('Found dogtoy...') # Got a dogtoy. Request pick up. # Stow the arm in case it is deployed stow_cmd = RobotCommandBuilder.arm_stow_command() command_client.robot_command(stow_cmd) # Walk to the object. walk_rt_vision, heading_rt_vision = compute_stand_location_and_yaw( vision_tform_dogtoy, robot_state_client, distance_margin=1.0) move_cmd = RobotCommandBuilder.trajectory_command( goal_x=walk_rt_vision[0], goal_y=walk_rt_vision[1], goal_heading=heading_rt_vision, frame_name=frame_helpers.VISION_FRAME_NAME, params=get_walking_params(0.5, 0.5)) end_time = 5.0 cmd_id = command_client.robot_command(command=move_cmd, end_time_secs=time.time() + end_time) # Wait until the robot reports that it is at the goal. block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True) # The ML result is a bounding box. Find the center. (center_px_x, center_px_y) = find_center_px(dogtoy.image_properties.coordinates) # Request Pick Up on that pixel. pick_vec = geometry_pb2.Vec2(x=center_px_x, y=center_px_y) grasp = manipulation_api_pb2.PickObjectInImage( pixel_xy=pick_vec, transforms_snapshot_for_camera=image.shot.transforms_snapshot, frame_name_image_sensor=image.shot.frame_name_image_sensor, camera_model=image.source.pinhole) # We can specify where in the gripper we want to grasp. About halfway is generally good for # small objects like this. For a bigger object like a shoe, 0 is better (use the entire # gripper) grasp.grasp_params.grasp_palm_to_fingertip = 0.6 # Tell the grasping system that we want a top-down grasp. # Add a constraint that requests that the x-axis of the gripper is pointing in the # negative-z direction in the vision frame. # The axis on the gripper is the x-axis. axis_on_gripper_ewrt_gripper = geometry_pb2.Vec3(x=1, y=0, z=0) # The axis in the vision frame is the negative z-axis axis_to_align_with_ewrt_vision = geometry_pb2.Vec3(x=0, y=0, z=-1) # Add the vector constraint to our proto. constraint = grasp.grasp_params.allowable_orientation.add() constraint.vector_alignment_with_tolerance.axis_on_gripper_ewrt_gripper.CopyFrom( axis_on_gripper_ewrt_gripper) constraint.vector_alignment_with_tolerance.axis_to_align_with_ewrt_frame.CopyFrom( axis_to_align_with_ewrt_vision) # We'll take anything within about 15 degrees for top-down or horizontal grasps. constraint.vector_alignment_with_tolerance.threshold_radians = 0.25 # Specify the frame we're using. grasp.grasp_params.grasp_params_frame_name = frame_helpers.VISION_FRAME_NAME # Build the proto grasp_request = manipulation_api_pb2.ManipulationApiRequest( pick_object_in_image=grasp) # Send the request print('Sending grasp request...') cmd_response = manipulation_api_client.manipulation_api_command( manipulation_api_request=grasp_request) # Wait for the grasp to finish grasp_done = False failed = False time_start = time.time() while not grasp_done: feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( manipulation_cmd_id=cmd_response.manipulation_cmd_id) # Send a request for feedback response = manipulation_api_client.manipulation_api_feedback_command( manipulation_api_feedback_request=feedback_request) current_state = response.current_state current_time = time.time() - time_start print('Current state ({time:.1f} sec): {state}'.format( time=current_time, state=manipulation_api_pb2.ManipulationFeedbackState.Name( current_state)), end=' \r') sys.stdout.flush() failed_states = [ manipulation_api_pb2.MANIP_STATE_GRASP_FAILED, manipulation_api_pb2. MANIP_STATE_GRASP_PLANNING_NO_SOLUTION, manipulation_api_pb2. MANIP_STATE_GRASP_FAILED_TO_RAYCAST_INTO_MAP, manipulation_api_pb2. MANIP_STATE_GRASP_PLANNING_WAITING_DATA_AT_EDGE ] failed = current_state in failed_states grasp_done = current_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED or failed time.sleep(0.1) holding_toy = not failed # Move the arm to a carry position. print('') print('Grasp finished, search for a person...') carry_cmd = RobotCommandBuilder.arm_carry_command() command_client.robot_command(carry_cmd) # Wait for the carry command to finish time.sleep(0.75) person = None while person is None: # Find a person to deliver the toy to person, image, vision_tform_person = get_obj_and_img( network_compute_client, options.ml_service, options.person_model, options.confidence_person, kImageSources, 'person') # We now have found a person to drop the toy off near. drop_position_rt_vision, heading_rt_vision = compute_stand_location_and_yaw( vision_tform_person, robot_state_client, distance_margin=2.0) wait_position_rt_vision, wait_heading_rt_vision = compute_stand_location_and_yaw( vision_tform_person, robot_state_client, distance_margin=3.0) # Tell the robot to go there # Limit the speed so we don't charge at the person. move_cmd = RobotCommandBuilder.trajectory_command( goal_x=drop_position_rt_vision[0], goal_y=drop_position_rt_vision[1], goal_heading=heading_rt_vision, frame_name=frame_helpers.VISION_FRAME_NAME, params=get_walking_params(0.5, 0.5)) end_time = 5.0 cmd_id = command_client.robot_command(command=move_cmd, end_time_secs=time.time() + end_time) # Wait until the robot reports that it is at the goal. block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True) print('Arrived at goal, dropping object...') # Do an arm-move to gently put the object down. # Build a position to move the arm to (in meters, relative to and expressed in the gravity aligned body frame). x = 0.75 y = 0 z = -0.25 hand_ewrt_flat_body = geometry_pb2.Vec3(x=x, y=y, z=z) # Point the hand straight down with a quaternion. qw = 0.707 qx = 0 qy = 0.707 qz = 0 flat_body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) flat_body_tform_hand = geometry_pb2.SE3Pose( position=hand_ewrt_flat_body, rotation=flat_body_Q_hand) robot_state = robot_state_client.get_robot_state() vision_tform_flat_body = frame_helpers.get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, frame_helpers.VISION_FRAME_NAME, frame_helpers.GRAV_ALIGNED_BODY_FRAME_NAME) vision_tform_hand_at_drop = vision_tform_flat_body * math_helpers.SE3Pose.from_obj( flat_body_tform_hand) # duration in seconds seconds = 1 arm_command = RobotCommandBuilder.arm_pose_command( vision_tform_hand_at_drop.x, vision_tform_hand_at_drop.y, vision_tform_hand_at_drop.z, vision_tform_hand_at_drop.rot.w, vision_tform_hand_at_drop.rot.x, vision_tform_hand_at_drop.rot.y, vision_tform_hand_at_drop.rot.z, frame_helpers.VISION_FRAME_NAME, seconds) # Keep the gripper closed. gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 0.0) # Combine the arm and gripper commands into one RobotCommand command = RobotCommandBuilder.build_synchro_command( gripper_command, arm_command) # Send the request cmd_id = command_client.robot_command(command) # Wait until the arm arrives at the goal. block_until_arm_arrives(command_client, cmd_id) # Open the gripper gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 1.0) command = RobotCommandBuilder.build_synchro_command(gripper_command) cmd_id = command_client.robot_command(command) # Wait for the dogtoy to fall out time.sleep(1.5) # Stow the arm. stow_cmd = RobotCommandBuilder.arm_stow_command() command_client.robot_command(stow_cmd) time.sleep(1) print('Backing up and waiting...') # Back up one meter and wait for the person to throw the object again. move_cmd = RobotCommandBuilder.trajectory_command( goal_x=wait_position_rt_vision[0], goal_y=wait_position_rt_vision[1], goal_heading=wait_heading_rt_vision, frame_name=frame_helpers.VISION_FRAME_NAME, params=get_walking_params(0.5, 0.5)) end_time = 5.0 cmd_id = command_client.robot_command(command=move_cmd, end_time_secs=time.time() + end_time) # Wait until the robot reports that it is at the goal. block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True) lease_client.return_lease(lease)
def start_spot_ros_interface(self): # ROS Node initialization rospy.init_node('spot_ros_interface_py') rate = rospy.Rate(200) # Update at 200 Hz # Each service will handle a specific command to Spot instance rospy.Service("self_right_cmd", spot_ros_srvs.srv.Stand, self.self_right_cmd_srv) rospy.Service("stand_cmd", spot_ros_srvs.srv.Stand, self.stand_cmd_srv) rospy.Service("trajectory_cmd", spot_ros_srvs.srv.Trajectory, self.trajectory_cmd_srv) rospy.Service("velocity_cmd", spot_ros_srvs.srv.Velocity, self.velocity_cmd_srv) # Single image publisher will publish all images from all Spot cameras kinematic_state_pub = rospy.Publisher( "kinematic_state", spot_ros_msgs.msg.KinematicState, queue_size=20) robot_state_pub = rospy.Publisher( "robot_state", spot_ros_msgs.msg.RobotState, queue_size=20) occupancy_grid_pub = rospy.Publisher( "occupancy_grid", visualization_msgs.msg.Marker, queue_size=20) # Publish tf2 from visual odometry frame to Spot's base link spot_tf_broadcaster = tf2_ros.TransformBroadcaster() # Publish static tf2 from Spot's base link to front-left camera spot_tf_static_broadcaster = tf2_ros.StaticTransformBroadcaster() image_only_pub = rospy.Publisher( "spot_image", sensor_msgs.msg.Image, queue_size=20) camera_info_pub = rospy.Publisher( "spot_cam_info", sensor_msgs.msg.CameraInfo, queue_size=20) # TODO: Publish depth images # depth_image_pub = rospy.Publisher( # "depth_image", sensor_msgs.msg.Image, queue_size=20) # For RViz 3rd person POV visualization if self.third_person_view: joint_state_pub = rospy.Publisher( "joint_state_from_spot", sensor_msgs.msg.JointState, queue_size=20) try: with bosdyn.client.lease.LeaseKeepAlive(self.lease_client), bosdyn.client.estop.EstopKeepAlive( self.estop_endpoint): rospy.loginfo("Acquired lease") if self.motors_on: rospy.loginfo("Powering on robot... This may take a several seconds.") self.robot.power_on(timeout_sec=20) assert self.robot.is_powered_on(), "Robot power on failed." rospy.loginfo("Robot powered on.") else: rospy.loginfo("Not powering on robot, continuing") while not rospy.is_shutdown(): ''' Publish Robot State''' kinematic_state, robot_state = self.get_robot_state() kinematic_state_pub.publish(kinematic_state) robot_state_pub.publish(robot_state) # Publish tf2 from the fixed vision_odometry_frame to the Spot's base_link t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "vision_odometry_frame" t.child_frame_id = "base_link" t.transform.translation.x = kinematic_state.vision_tform_body.translation.x t.transform.translation.y = kinematic_state.vision_tform_body.translation.y t.transform.translation.z = kinematic_state.vision_tform_body.translation.z t.transform.rotation.x = kinematic_state.vision_tform_body.rotation.x t.transform.rotation.y = kinematic_state.vision_tform_body.rotation.y t.transform.rotation.z = kinematic_state.vision_tform_body.rotation.z t.transform.rotation.w = kinematic_state.vision_tform_body.rotation.w spot_tf_broadcaster.sendTransform(t) if self.third_person_view: joint_state_pub.publish(kinematic_state.joint_states) ''' Publish Images''' img_reqs = [image_pb2.ImageRequest(image_source_name=source, image_format=image_pb2.Image.FORMAT_RAW) for source in self.image_source_names[2:3]] image_list = self.image_client.get_image(img_reqs) for img in image_list: if img.status == image_pb2.ImageResponse.STATUS_OK: header = std_msgs.msg.Header() header.stamp = t.header.stamp header.frame_id = img.source.name if img.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: dtype = np.uint16 else: dtype = np.uint8 if img.shot.image.format == image_pb2.Image.FORMAT_RAW: image = np.fromstring(img.shot.image.data, dtype=dtype) image = image.reshape(img.shot.image.rows, img.shot.image.cols) # Make Image component of ImageCapture i = sensor_msgs.msg.Image() i.header = header i.width = img.shot.image.cols i.height = img.shot.image.rows i.data = img.shot.image.data if img.shot.image.format != image_pb2.Image.FORMAT_RAW else image.tobytes() i.step = img.shot.image.cols i.encoding = 'mono8' # CameraInfo cam_info = sensor_msgs.msg.CameraInfo() cam_info.header = i.header cam_info.width = i.width cam_info.height = i.height cam_info.distortion_model = "plumb_bob" cam_info.D = [0.0,0.0,0.0,0.0] f = img.source.pinhole.intrinsics.focal_length c = img.source.pinhole.intrinsics.principal_point cam_info.K = \ [f.x, 0, c.x, \ 0, f.y, c.y, \ 0, 0, 1] # Transform from base_link to camera for current img body_tform_cam = get_a_tform_b(img.shot.transforms_snapshot, BODY_FRAME_NAME, img.shot.frame_name_image_sensor) # Generate camera to body Transform body_tform_cam_tf = geometry_msgs.msg.Transform() body_tform_cam_tf.translation.x = body_tform_cam.position.x body_tform_cam_tf.translation.y = body_tform_cam.position.y body_tform_cam_tf.translation.z = body_tform_cam.position.z body_tform_cam_tf.rotation.x = body_tform_cam.rotation.x body_tform_cam_tf.rotation.y = body_tform_cam.rotation.y body_tform_cam_tf.rotation.z = body_tform_cam.rotation.z body_tform_cam_tf.rotation.w = body_tform_cam.rotation.w camera_transform_stamped = geometry_msgs.msg.TransformStamped() camera_transform_stamped.header.stamp = header.stamp camera_transform_stamped.header.frame_id = "base_link" camera_transform_stamped.transform = body_tform_cam_tf camera_transform_stamped.child_frame_id = img.source.name # Publish body to camera static tf spot_tf_static_broadcaster.sendTransform(camera_transform_stamped) # Publish current image and camera info image_only_pub.publish(i) camera_info_pub.publish(cam_info) ''' Publish occupancy grid''' if occupancy_grid_pub.get_num_connections() > 0: local_grid_proto = self.grid_client.get_local_grids(['terrain']) markers = get_terrain_markers(local_grid_proto) occupancy_grid_pub.publish(markers) rospy.logdebug("Looping...") rate.sleep() finally: # If we successfully acquired a lease, return it. self.lease_client.return_lease(self.lease)
def test_frame_tree_math_big_tree(): snapshot_text = """child_to_parent_edge_map { key: "beta" value: { parent_frame_name: "alpha" parent_tform_child: { position: { x: 10 y: 0 z: 0 } } } } child_to_parent_edge_map { key: "gamma" value: { parent_frame_name: "alpha" parent_tform_child: { position: { x: 0 y: 0 z: 10 } } } } child_to_parent_edge_map { key: "delta" value: { parent_frame_name: "beta" parent_tform_child: { position: { x: 100 y: 0 z: 0 } } } } child_to_parent_edge_map { key: "epsilon" value: { parent_frame_name: "beta" parent_tform_child: { position: { x: 1000 y: 0 z: 0 } } } } child_to_parent_edge_map { key: "zeta" value: { parent_frame_name: "gamma" parent_tform_child: { position: { x: 0 y: 0 z: 100 } } } } child_to_parent_edge_map { key: "eta" value: { parent_frame_name: "gamma" parent_tform_child: { position: { x: 0 y: 0 z: 1000 } } } } child_to_parent_edge_map { key: "alpha" value: { parent_frame_name: "" } }""" frame_tree = _create_snapshot(snapshot_text) assert frame_helpers.validate_frame_tree_snapshot(frame_tree) # alpha as source frame assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'alpha')) assert _do_poses_match( 10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'beta')) assert _do_poses_match( 0, 0, 10, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'gamma')) assert _do_poses_match( 110, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'delta')) assert _do_poses_match( 1010, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'epsilon')) assert _do_poses_match( 0, 0, 110, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'zeta')) assert _do_poses_match( 0, 0, 1010, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'eta')) # beta as source frame assert _do_poses_match( -10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'alpha')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'beta')) assert _do_poses_match( -10, 0, 10, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'gamma')) assert _do_poses_match( 100, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'delta')) assert _do_poses_match( 1000, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'epsilon')) assert _do_poses_match( -10, 0, 110, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'zeta')) assert _do_poses_match( -10, 0, 1010, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'eta')) # gamma as source frame assert _do_poses_match( 0, 0, -10, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'alpha')) assert _do_poses_match( 10, 0, -10, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'beta')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'gamma')) assert _do_poses_match( 110, 0, -10, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'delta')) assert _do_poses_match( 1010, 0, -10, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'epsilon')) assert _do_poses_match( 0, 0, 100, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'zeta')) assert _do_poses_match( 0, 0, 1000, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'eta')) # delta as source frame assert _do_poses_match( -110, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'alpha')) assert _do_poses_match( -100, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'beta')) assert _do_poses_match( -110, 0, 10, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'gamma')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'delta')) assert _do_poses_match( 900, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'epsilon')) assert _do_poses_match( -110, 0, 110, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'zeta')) assert _do_poses_match( -110, 0, 1010, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'eta')) # epsilon as source frame assert _do_poses_match( -1010, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'alpha')) assert _do_poses_match( -1000, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'beta')) assert _do_poses_match( -1010, 0, 10, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'gamma')) assert _do_poses_match( -900, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'delta')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'epsilon')) assert _do_poses_match( -1010, 0, 110, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'zeta')) assert _do_poses_match( -1010, 0, 1010, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'eta')) # zeta as source frame assert _do_poses_match( 0, 0, -110, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'alpha')) assert _do_poses_match( 10, 0, -110, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'beta')) assert _do_poses_match( 0, 0, -100, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'gamma')) assert _do_poses_match( 110, 0, -110, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'delta')) assert _do_poses_match( 1010, 0, -110, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'epsilon')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'zeta')) assert _do_poses_match( 0, 0, 900, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'eta')) # eta as source frame assert _do_poses_match( 0, 0, -1010, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'alpha')) assert _do_poses_match( 10, 0, -1010, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'beta')) assert _do_poses_match( 0, 0, -1000, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'gamma')) assert _do_poses_match( 110, 0, -1010, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'delta')) assert _do_poses_match( 1010, 0, -1010, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'epsilon')) assert _do_poses_match( 0, 0, -900, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'zeta')) assert _do_poses_match( 0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'eta'))
def open_door(robot, request_manager, snapshot): """Command the robot to automatically open a door via the door service API. Args: robot: (Robot) Interface to Spot robot. request_manager: (RequestManager) Object for bookkeeping user touch points. snapshot: (TransformSnapshot) Snapshot from the WalkToObjectInImage command which contains the 3D location reported from a raycast based on the user hinge touch point. """ robot.logger.info("Opening door...") # Using the raycast intersection point and the vision_tform_raycast = frame_helpers.get_a_tform_b( snapshot, frame_helpers.VISION_FRAME_NAME, frame_helpers.RAYCAST_FRAME_NAME) vision_tform_sensor = request_manager.vision_tform_sensor raycast_point_wrt_vision = vision_tform_raycast.get_translation() ray_from_camera_to_object = raycast_point_wrt_vision - vision_tform_sensor.get_translation( ) ray_from_camera_to_object_norm = np.sqrt( np.sum(ray_from_camera_to_object**2)) ray_from_camera_normalized = ray_from_camera_to_object / ray_from_camera_to_object_norm auto_cmd = door_pb2.DoorCommand.AutoGraspCommand() auto_cmd.frame_name = frame_helpers.VISION_FRAME_NAME search_dist_meters = 0.25 search_ray = search_dist_meters * ray_from_camera_normalized search_ray_start_in_frame = raycast_point_wrt_vision - search_ray auto_cmd.search_ray_start_in_frame.CopyFrom( geometry_pb2.Vec3(x=search_ray_start_in_frame[0], y=search_ray_start_in_frame[1], z=search_ray_start_in_frame[2])) search_ray_end_in_frame = raycast_point_wrt_vision + search_ray auto_cmd.search_ray_end_in_frame.CopyFrom( geometry_pb2.Vec3(x=search_ray_end_in_frame[0], y=search_ray_end_in_frame[1], z=search_ray_end_in_frame[2])) auto_cmd.hinge_side = request_manager.hinge_side auto_cmd.swing_direction = door_pb2.DoorCommand.SWING_DIRECTION_UNKNOWN door_command = door_pb2.DoorCommand.Request(auto_grasp_command=auto_cmd) request = door_pb2.OpenDoorCommandRequest(door_command=door_command) # Command the robot to open the door. door_client = robot.ensure_client(DoorClient.default_service_name) response = door_client.open_door(request) feedback_request = door_pb2.OpenDoorFeedbackRequest() feedback_request.door_command_id = response.door_command_id timeout_sec = 60.0 end_time = time.time() + timeout_sec while time.time() < end_time: feedback_response = door_client.open_door_feedback(feedback_request) if (feedback_response.status != basic_command_pb2.RobotCommandFeedbackStatus.STATUS_PROCESSING ): raise Exception("Door command reported status ") if (feedback_response.feedback.status == door_pb2.DoorCommand.Feedback.STATUS_COMPLETED): robot.logger.info("Opened door.") return time.sleep(0.5) raise Exception("Door command timed out. Try repositioning the robot.")