def _do_detection(self, point_head_at):
req = TabletopDetectionRequest()
req.return_clusters = True
req.return_models = True
req.num_models = 1
if point_head_at:
point_head_at = tl.transform_point_stamped(self._wi.world_frame, point_head_at)
zrng = range(0, int(100*self.table_search_max), int(100*self.table_search_resolution))
sgnrng = [-1.0, 1.0]
rospy.logdebug('For table finding, trying z offsets for head of:' +str(zrng))
if not point_head_at or not zrng:
zrng = [0]
sgnrng = [1.0]
first_pass=True
for z in zrng:
for sgn in sgnrng:
if point_head_at:
new_point = copy.copy(point_head_at.point)
new_point.z += sgn*z/100.0
self._head._point_head([new_point.x, new_point.y, new_point.z], point_head_at.header.frame_id)
rospy.sleep(0.4) #let the head bobbing settle out
res = self._detect_srv(req)
if res.detection.result != res.detection.SUCCESS:
rospy.logerr('Unable to find table. Error was: '+ str(res.detection.result))
if first_pass:
exc = DetectionError('Unable to find table', error_code=res.detection.result)
first_pass = False
continue
if self.allow_vertical_tables:
return res
table = res.detection.table
#is the table somewhat horizontal?
table_pose_bl = tl.transform_pose_stamped(self._wi.robot_frame, table.pose)
vert = Point()
vert.z = 1.0
oripose = Pose()
oripose.orientation = table_pose_bl.pose.orientation
table_vertical = gt.transform_point(vert, oripose)
if table_vertical.z < self.vertical_threshold:
rospy.logerr('Table vertical is [%f, %f, %f]. This is not a horizontal table',
table_vertical.x, table_vertical.y, table_vertical.z)
res.detection.result = res.detection.OTHER_ERROR
if first_pass:
exc = DetectionError('Unable to find horizontal table', error_code=res.detection.result)
first_pass = False
continue
rospy.loginfo('Found horizontal table. Vertical is [%f, %f, %f]',
table_vertical.x, table_vertical.y, table_vertical.z)
return res
raise exc
def transform_point(self, new_frame_id, old_frame_id, point, robot_state):
"""
Transforms a point defined in old_frame_id into new_frame_id according to robot state.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**point (geometry_msgs.msg.Point):** point defined in old_frame_id
**robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation
**Returns:**
A geometry_msgs.msg.Point defined in new_frame_id
**Raises:**
**exceptions.TransformStateError:** if there is an error getting the transform
"""
trans = self.get_transform(old_frame_id, new_frame_id, robot_state)
return gt.transform_point(point, conv.transform_to_pose(trans.transform))
def transform_point(self, new_frame_id, old_frame_id, point, robot_state):
'''
Transforms a point defined in old_frame_id into new_frame_id according to robot state.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**point (geometry_msgs.msg.Point):** point defined in old_frame_id
**robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation
**Returns:**
A geometry_msgs.msg.Point defined in new_frame_id
**Raises:**
**exceptions.TransformStateError:** if there is an error getting the transform
'''
trans = self.get_transform(old_frame_id, new_frame_id, robot_state)
return gt.transform_point(point,
conv.transform_to_pose(trans.transform))
def max_angles(self, pose):
'''
@type pose: PoseStamped
@param pose: the pose the object will be released in (in the frame of the robot)
'''
## get the center of mass in the frame of the object,
## then transform it into the robot frame
cm = self.center_of_mass(pose).tolist() ## in the frame of the object
tf_cm = self.translate(cm, pose)
## get the corners of the base of the bounding box of the object in robot frame
base = self.get_base(pose.pose)
# print "base="+str([pivot.position.x, pivot.position.y, pivot.position.z])
print "base=" + str(base)
## corner of the base is the pivot pt, transform to obj frame
# pivot_matrix = self.inv_translate([[base[3][0]],[base[3][1]],[base[3][2]]], pose).tolist()
# pivot = [pivot_matrix[0][0], pivot_matrix[1][0], pivot_matrix[2][0]]
## get the euler angles of the pose (goal pose)
(orig_phi, orig_theta,
orig_psi) = geometry_tools.quaternion_to_euler(pose.pose.orientation)
## max rotation of pi/2
max_phi = orig_phi + math.pi / 2
max_theta = orig_theta + math.pi / 2
pivot = Point()
pivot.x = base[0][0]
pivot.y = base[0][1]
pivot.z = base[0][2]
cm = Pose()
cm.position.x = tf_cm[0][0]
cm.position.y = tf_cm[1][0]
cm.position.z = tf_cm[2][0]
cm.orientation = pose.pose.orientation
t = geometry_tools.inverse_transform_point(pivot, cm)
tol = 0.01
## find max angle around x-axis, phi
print "cm=" + str(tf_cm.tolist())
for phi in frange(0.0, math.pi / 2, 0.1):
transform = Pose()
transform.orientation = geometry_tools.euler_to_quaternion(
phi, 0.0, 0.0)
cm_new = Pose()
cm_new.position = cm.position
cm_new.orientation = geometry_tools.transform_quaternion(
cm.orientation, transform)
p_new = geometry_tools.transform_point(t, cm_new)
y_diff = abs(tf_cm[1][0] - p_new.y)
z_diff = abs(tf_cm[2][0] - p_new.z)
# print "phi angle="+str(phi)+" pivot="+str([p_new.x, p_new.y, p_new.z])
if y_diff < tol or z_diff < tol:
max_phi = phi
break
for theta in frange(0.0, math.pi / 2, 0.1):
transform = Pose()
transform.orientation = geometry_tools.euler_to_quaternion(
0.0, theta, 0.0)
cm_new = Pose()
cm_new.position = cm.position
cm_new.orientation = geometry_tools.transform_quaternion(
cm.orientation, transform)
p_new = geometry_tools.transform_point(t, cm_new)
x_diff = abs(tf_cm[0][0] - p_new.x)
z_diff = abs(tf_cm[2][0] - p_new.z)
# print "theta angle="+str(theta)+" pivot="+str([p_new.x, p_new.y, p_new.z])
if z_diff < tol or x_diff < tol:
max_theta = theta
break
return (max_phi, max_theta)
def free_spots_from_dimensions(table, object_dims, blocking_objs, res=0.1):
'''
Currently works only with a single shape
Assumes table is in xy plane
'''
#find the lower left corner of the table in the header frame
#we want everything relative to this point
table_corners = gt.bounding_box_corners(table.shapes[0])
lower_left = Pose()
lower_left.position = gt.list_to_point(table_corners[0])
lower_left.orientation.w = 1.0
#rospy.loginfo('Table corners are:')
#for c in table_corners: rospy.loginfo('\n'+str(c))
#rospy.loginfo('Lower left =\n'+str(lower_left))
#rospy.loginfo('Table header =\n'+str(table.header))
#rospy.loginfo('Table pose =\n'+str(table.poses[0]))
#this is the position of the minimum x, minimum y point in the table's header frame
table_box_origin = gt.transform_pose(lower_left, table.poses[0])
tr = gt.inverse_transform_list(gt.transform_list(table_corners[-1], table.poses[0]), table_box_origin)
table_dims = (tr[0], tr[1])
tbos = PoseStamped()
tbos.header = table.header
tbos.pose = table_box_origin
marray.markers.append(vt.marker_at(tbos, ns='table_origin', mtype=Marker.CUBE, r=1.0))
max_box = PointStamped()
max_box.header = table.header
max_box.point = gt.list_to_point(gt.transform_list([table_dims[0], table_dims[1], 0], table_box_origin))
marray.markers.append(vt.marker_at_point(max_box, ns='table_max', mtype=Marker.CUBE, r=1.0))
#rospy.loginfo('Table box origin is '+str(table_box_origin)+' dimensions are '+str(table_dims))
locs_on_table = _get_feasible_locs(table_dims, object_dims, res)
for i, l in enumerate(locs_on_table):
pt = Point()
pt.x = l[0]
pt.y = l[1]
mpt = PointStamped()
mpt.header = table.header
mpt.point = gt.transform_point(pt, table_box_origin)
marray.markers.append(vt.marker_at_point(mpt, mid=i, ns='locations', r=1.0, g=1.0, b=0.0))
feasible_locs = []
#check that these points really are on the table
for i, l in enumerate(locs_on_table):
pt = Point()
pt.x = l[0]
pt.y = l[1]
#this point is now defined relative to the origin of the table (rather than its minimum x, minimum y point)
table_pt = gt.inverse_transform_point(gt.transform_point(pt, table_box_origin), table.poses[0])
if point_on_table(table_pt, table.shapes[0]):
feasible_locs.append(l)
marray.markers[i+2].color.r = 0.0
marray.markers[i+2].color.b = 1.0
rospy.loginfo('Testing '+str(len(feasible_locs))+' locations')
if not feasible_locs:
return feasible_locs
forbidden=[]
for i, o in enumerate(blocking_objs):
ofcs = gt.bounding_box_corners(o.shapes[0])
objpose = tl.transform_pose(table.header.frame_id, o.header.frame_id, o.poses[0])
hfcs = [gt.transform_list(c, objpose) for c in ofcs]
tfcs = [gt.inverse_transform_list(c, table_box_origin) for c in hfcs]
oxmax = max([c[0] for c in tfcs])
oxmin = min([c[0] for c in tfcs])
oymax = max([c[1] for c in tfcs])
oymin = min([c[1] for c in tfcs])
forbidden.append(((oxmin, oymin), (oxmax - oxmin, oymax - oymin)))
#rospy.loginfo('\n'+str(forbidden[-1]))
ps = PoseStamped()
ps.header = table.header
ps.pose = objpose
ps = PoseStamped()
ps.header = table.header
ps.pose.position = gt.list_to_point(gt.transform_list([oxmin, oymin, 0], table_box_origin))
ps.pose.orientation.w = 1.0
marray.markers.append(vt.marker_at(ps, ns='forbidden', mid=i, r=1.0, b=0.0))
# Remove forbidden rectangles
for (bottom_left, dims) in forbidden:
_remove_feasible_locs(feasible_locs, object_dims,
bottom_left,
_add(bottom_left, dims),
res)
rospy.loginfo('There are '+str(len(feasible_locs))+' possible locations')
obj_poses = []
for i, fl in enumerate(feasible_locs):
table_frame_pose = Pose()
table_frame_pose.position.x = fl[0] + object_dims[0]/2.0
table_frame_pose.position.y = fl[1] + object_dims[1]/2.0
table_frame_pose.orientation.w = 1.0
pose = PoseStamped()
pose.header = copy.deepcopy(table.header)
pose.pose = gt.transform_pose(table_frame_pose, table_box_origin)
obj_poses.append(pose)
pt = PointStamped()
pt.header = table.header
pt.point = pose.pose.position
marray.markers.append(vt.marker_at_point(pt, mid=i, ns='final_locations', g=1.0, b=0.0))
#rospy.loginfo('Object poses are:')
#for op in obj_poses: rospy.loginfo(str(op))
for i in range(10):
vpub.publish(marray)
rospy.sleep(0.1)
return obj_poses
