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.")
