def decode_to_pose_array_msg(self,
ref_frame,
ref_frame_id=None,
scaling_factor=1.0):
"""Decode the bytes in the streaming data to pose array message.
:param ref_frame: str Reference frame name of the generated pose array message.
:param ref_frame_id: None/int If not None, all poses will be shifted subject to
the frame with this ID. This frame should belong to the human.
:param scaling_factor: float Scale the position of the pose if src_frame_id is not None.
Its value equals to the robot/human body dimension ratio
"""
pose_array_msg = GeometryMsg.PoseArray()
pose_array_msg.header.stamp = rospy.Time.now()
pose_array_msg.header.frame_id = ref_frame
for i in range(self.item_num):
item = self.payload[i * self._item_size:(i + 1) * self._item_size]
pose_msg = self._type_02_decode_to_pose_msg(item)
if pose_msg is None:
return None
pose_array_msg.poses.append(pose_msg)
if ref_frame_id is not None and ref_frame_id < len(
pose_array_msg.poses):
relative_pose_array_msg = GeometryMsg.PoseArray()
relative_pose_array_msg.header.frame_id = ref_frame
reference_pose = pose_array_msg.poses[ref_frame_id]
for p in pose_array_msg.poses:
std_relative_pose = common.get_transform_same_origin(
reference_pose, p)
relative_pose = common.to_ros_pose(std_relative_pose)
relative_pose_array_msg.poses.append(relative_pose)
return relative_pose_array_msg
return pose_array_msg
def _handles_loop(self):
"""
For each handle in HandleListMsg,
calculate average pose
"""
rospy.sleep(5)
while not rospy.is_shutdown() and not self.exited:
rospy.sleep(.01)
handle_list_msg = self.handle_list_msg
pose_array = gm.PoseArray()
pose_array.header.frame_id = handle_list_msg.header.frame_id
pose_array.header.stamp = rospy.Time.now()
avg_pose_array = gm.PoseArray()
avg_pose_array.header.frame_id = handle_list_msg.header.frame_id
avg_pose_array.header.stamp = rospy.Time.now()
cam_to_base = tfx.lookupTransform('base_link', handle_list_msg.header.frame_id).matrix[:3,:3]
switch = np.matrix([[0, 1, 0],
[1, 0, 0],
[0, 0, 1]])
for handle in handle_list_msg.handles:
all_poses = [tfx.pose(cylinder.pose, stamp=rospy.Time.now(), frame=handle_list_msg.header.frame_id) for cylinder in handle.cylinders]
rotated_poses = [tfx.pose(p.position, tfx.tb_angles(p.orientation.matrix*switch)) for p in all_poses]
filtered_poses = list()
for rot_pose in rotated_poses:
r_base = cam_to_base*rot_pose.orientation.matrix
if r_base[0,0] > 0:
if r_base[2,2] > 0:
rot_pose.orientation = tfx.tb_angles(rot_pose.orientation.matrix*tfx.tb_angles(0,0,180).matrix)
filtered_poses.append(rot_pose)
pose_array.poses += [pose.msg.Pose() for pose in filtered_poses]
if len(filtered_poses) > 0:
avg_position = sum([p.position.array for p in filtered_poses])/float(len(filtered_poses))
avg_quat = sum([p.orientation.quaternion for p in filtered_poses])/float(len(filtered_poses))
avg_pose_array.poses.append(tfx.pose(avg_position, avg_quat).msg.Pose())
if len(pose_array.poses) > 0:
self.handles_pose_pub.publish(pose_array)
self.avg_handles_pose_pub.publish(avg_pose_array)
def draw_curve(arr):
pose_array = gmm.PoseArray()
for (x, y, z) in arr:
pose = gmm.Pose()
pose.position = gmm.Point(x, y, z)
pose_array.poses.append(pose)
pose_array.header.frame_id = "/base_footprint"
pose_array.header.stamp = rospy.Time.now()
rviz.draw_curve(pose_array)
def to_ros_poses(poses):
msg = GeometryMsg.PoseArray()
if isinstance(poses, np.ndarray):
for pose in poses:
ros_pose = to_ros_pose(pose)
msg.poses.append(ros_pose)
return msg
else:
raise NotImplementedError
def array_to_pose_array(xyz_arr, frame_id, quat_arr=None):
assert quat_arr is None or len(xyz_arr) == len(quat_arr)
pose_array = gm.PoseArray()
for index, xyz in enumerate(xyz_arr):
pose = gm.Pose()
pose.position = gm.Point(*xyz)
pose.orientation = gm.Quaternion(0,0,0,1) if quat_arr is None else gm.Quaternion(*(quat_arr[index]))
pose_array.poses.append(pose)
pose_array.header.frame_id = frame_id
pose_array.header.stamp = rospy.Time.now()
return pose_array
def handle_initialization_request(req):
"rope initialization service"
xyz, _ = pc2xyzrgb(req.cloud)
xyz = np.squeeze(xyz)
obj_type = determine_object_type(xyz, plotting=False)
print "object type:", obj_type
if obj_type == "towel":
xyz = xyz[(xyz[:, 2] - np.median(xyz[:, 2])) < .05, :]
center, (height, width), angle = cv2.minAreaRect(
xyz[:, :2].astype('float32').reshape(1, -1, 2))
angle *= np.pi / 180
corners = np.array(center)[None, :] + np.dot(
np.array([[-height / 2, -width / 2], [-height / 2, width / 2],
[height / 2, width / 2], [height / 2, -width / 2]]),
np.array([[np.cos(angle), np.sin(angle)],
[-np.sin(angle), np.cos(angle)]]))
resp = bs.InitializationResponse()
resp.objectInit.type = "towel_corners"
resp.objectInit.towel_corners.polygon.points = [
gm.Point(corner[0], corner[1], np.median(xyz[:, 2]))
for corner in corners
]
resp.objectInit.towel_corners.header = req.cloud.header
print req.cloud.header
poly_pub.publish(resp.objectInit.towel_corners)
if obj_type == "rope":
total_path_3d = find_path_through_point_cloud(xyz, plotting=False)
resp = bs.InitializationResponse()
resp.objectInit.type = "rope"
rope = resp.objectInit.rope = bm.Rope()
rope.header = req.cloud.header
rope.nodes = [gm.Point(x, y, z) for (x, y, z) in total_path_3d]
rope.radius = .006
rospy.logwarn(
"TODO: actually figure out rope radius from data. setting to .4cm")
pose_array = gm.PoseArray()
pose_array.header = req.cloud.header
pose_array.poses = [
gm.Pose(position=point, orientation=gm.Quaternion(0, 0, 0, 1))
for point in rope.nodes
]
marker_handles[0] = rviz.draw_curve(pose_array, 0)
return resp
def create_normals_pose_array(pts, normals, frame_id=''):
'''
creates a pose array for visualizing the normals
pts: Nx3 array of points
normals: Nx3 array of corresponding normals
'''
def compute_pose(c, n):
v = np.cross([1, 0, 0], n)
v /= np.linalg.norm(v)
th = ru.angle_between([1, 0, 0], n)
return geometry_msgs.Pose(geometry_msgs.Point(*c),
tr.axis_angle2rosq(v, th))
pa = geometry_msgs.PoseArray()
pa.header.frame_id = frame_id
pa.poses = map(lambda i: compute_pose(pts[i, :], normals[i, :]),
np.arange(pts.shape[0]))
return pa
def execute(self, ud):
table = ud['table']
table_pose = ud['table_pose'] # expected on map frame, as robot_pose
if not table or not table_pose:
rospy.logerr("Detected table contains None!")
return 'no_table'
p_x = self.distance + table.depth / 2.0
n_x = -p_x
p_y = self.distance + table.width / 2.0
n_y = -p_y
table_tf = to_transform(table_pose, 'table_frame')
TF2().publish_transform(table_tf)
table_tf.transform.translation.z = 0.0
closest_pose = None
closest_dist = float('inf')
from math import pi
poses = [('p_x', create_2d_pose(p_x, 0.0, -pi, 'table_frame')),
('n_x', create_2d_pose(n_x, 0.0, 0.0, 'table_frame')),
('p_y', create_2d_pose(0.0, p_y, -pi / 2.0, 'table_frame')),
('n_y', create_2d_pose(0.0, n_y, +pi / 2.0, 'table_frame'))]
pose_array = geometry_msgs.PoseArray() # for visualization
sides_poses = {}
for name, pose in poses:
pose = transform_pose(pose, table_tf)
pose_array.poses.append(deepcopy(pose.pose))
pose_array.poses[
-1].position.z += 0.025 # raise over costmap to make it visible
sides_poses[name] = pose
dist = distance_2d(pose, ud['robot_pose'])
if dist < closest_dist:
closest_pose = pose
closest_dist = dist
ud['poses'] = sides_poses
ud['closest_pose'] = closest_pose
pose_array.header = deepcopy(closest_pose.header)
pose_array.poses.append(deepcopy(closest_pose.pose))
pose_array.poses[
-1].position.z += 0.05 # remark the closest pose with a double arrow
self.poses_viz.publish(pose_array)
return 'succeeded'
def _handles_loop(self):
"""
For each handle in HandleListMsg,
calculate average pose
"""
while not rospy.is_shutdown():
self.handle_list_msg = None
self.camera_pose = None
while not rospy.is_shutdown() and (self.handle_list_msg is None
or self.camera_pose is None):
rospy.sleep(.01)
handle_list_msg = self.handle_list_msg
camera_pose = self.camera_pose
pose_array = gm.PoseArray()
pose_array.header.frame_id = 'base_link'
pose_array.header.stamp = rospy.Time.now()
avg_pose_array = gm.PoseArray()
avg_pose_array.header.frame_id = 'base_link'
avg_pose_array.header.stamp = rospy.Time.now()
if handle_list_msg.header.frame_id.count('base_link') > 0:
cam_to_base = np.eye(4)
else:
cam_to_base = camera_pose.matrix #tfx.lookupTransform('base_link', handle_list_msg.header.frame_id).matrix
switch = np.matrix([[0, 1, 0], [1, 0, 0], [0, 0, 1]])
for handle in handle_list_msg.handles:
all_poses = [
tfx.pose(cylinder.pose,
stamp=rospy.Time.now(),
frame=handle_list_msg.header.frame_id)
for cylinder in handle.cylinders
]
rotated_poses = [
tfx.pose(p.position,
tfx.tb_angles(p.orientation.matrix * switch))
for p in all_poses
]
filtered_poses = list()
for rot_pose in rotated_poses:
r_base = cam_to_base[:3, :3] * rot_pose.orientation.matrix
if r_base[0, 0] > 0:
if r_base[2, 2] > 0:
rot_pose.orientation = tfx.tb_angles(
rot_pose.orientation.matrix *
tfx.tb_angles(0, 0, 180).matrix)
filtered_poses.append(rot_pose)
filtered_poses = [
tfx.pose(cam_to_base * pose.matrix, frame='base_link')
for pose in filtered_poses
]
filtered_poses = filter(
lambda pose: min(self.min_positions < pose.position.array)
and min(pose.position.array < self.max_positions),
filtered_poses)
pose_array.poses += [
pose.msg.Pose() for pose in filtered_poses
]
if len(filtered_poses) > 0:
avg_position = sum(
[p.position.array
for p in filtered_poses]) / float(len(filtered_poses))
avg_quat = sum(
[p.orientation.quaternion
for p in filtered_poses]) / float(len(filtered_poses))
avg_pose_array.poses.append(
tfx.pose(avg_position, avg_quat).msg.Pose())
self.handles_pose_pub.publish(pose_array)
self.avg_handles_pose_pub.publish(avg_pose_array)
self.logger_pub.publish('handles {0}'.format(
len(avg_pose_array.poses)))
pi = 0
prevCoord = contour[0, 0, 0]
sign = np.sign(contour[0, 0, 1] - prevCoord)
for i in xrange(1, contour.shape[0]):
if (sign * (contour[i, 0, 0] - prevCoord) < 0):
subcontours.append(contour[pi:i, 0])
sign = np.sign(contour[i, 0, 0] - prevCoord)
pi = i
prevCoord = contour[i, 0, 0]
# TODO filter subcontours by array size
size = len(subcontours)
try:
for num in xrange(size):
trajectory = gmsgs.PoseArray()
# simple filter
if (subcontours[num].shape[0] <= 4):
# Debug information
# print("Shape {0}".format(subcontours[num].shape[0]))
# print(subcontours[num][:, 0]) # x
# print(subcontours[num][:, 1]) # y
continue
x = (subcontours[num][:, 0] - imgWidth / 2) * kx
y = (imgHeight / 2 - subcontours[num][:, 1]) * ky
# plt.plot(x, y)
# Fill trajectory message
for p in xrange(x.shape[0]):
pose = gmsgs.Pose()
def handle_initialization_request(req):
"rope initialization service"
if args.save_requests:
fname = "/tmp/init_req_%i.pkl" % time.time()
with open(fname, "w") as fh:
cPickle.dump(req, fh)
print "saved", fname
xyz, _ = pc2xyzrgb(req.cloud)
xyz = np.squeeze(xyz)
obj_type = determine_object_type(xyz, plotting=args.plotting)
print "object type:", obj_type
if obj_type == "towel":
xyz = xyz[(xyz[:, 2] - np.median(xyz[:, 2])) < .05, :]
center, (height, width), angle = cv2.minAreaRect(
xyz[:, :2].astype('float32').reshape(1, -1, 2))
angle *= np.pi / 180
corners = np.array(center)[None, :] + np.dot(
np.array([[-height / 2, -width / 2], [-height / 2, width / 2],
[height / 2, width / 2], [height / 2, -width / 2]]),
np.array([[np.cos(angle), np.sin(angle)],
[-np.sin(angle), np.cos(angle)]]))
resp = bs.InitializationResponse()
resp.objectInit.type = "towel_corners"
resp.objectInit.towel_corners.polygon.points = [
gm.Point(corner[0], corner[1], np.median(xyz[:, 2]))
for corner in corners
]
resp.objectInit.towel_corners.header = req.cloud.header
print req.cloud.header
poly_pub.publish(resp.objectInit.towel_corners)
if obj_type == "rope":
total_path_3d = find_path_through_point_cloud(xyz,
plotting=args.plotting)
resp = bs.InitializationResponse()
resp.objectInit.type = "rope"
rope = resp.objectInit.rope = bm.Rope()
rope.header = req.cloud.header
rope.nodes = [gm.Point(x, y, z) for (x, y, z) in total_path_3d]
rope.radius = .006
print "lengths:", [
np.linalg.norm(total_path_3d[i + 1] - total_path_3d[i])
for i in xrange(len(total_path_3d) - 1)
]
rospy.loginfo(
"created a rope with %i nodes, each has length %.2f, radius %.2f",
len(rope.nodes),
np.linalg.norm(total_path_3d[i + 1] - total_path_3d[i]),
rope.radius)
pose_array = gm.PoseArray()
pose_array.header = req.cloud.header
pose_array.poses = [
gm.Pose(position=point, orientation=gm.Quaternion(0, 0, 0, 1))
for point in rope.nodes
]
# marker_handles[0] = rviz.draw_curve(pose_array,0)
return resp
def main():
rospy.init_node('smach_block_manip')
sm = smach.StateMachine(outcomes=['success', 'aborted', 'preempted'])
with sm:
# general
sm.userdata.true = True
sm.userdata.false = False
# app config
sm.userdata.frame = rospy.get_param('~arm_link', 'arm_link')
sm.userdata.gripper_open = rospy.get_param('~gripper_open', 0.042)
sm.userdata.gripper_closed = rospy.get_param('~gripper_closed', 0.024)
sm.userdata.z_up = rospy.get_param('~z_up', 0.12)
sm.userdata.table_height = rospy.get_param('~table_height', 0.1)
sm.userdata.block_size = rospy.get_param('~block_size', 0.025)
sm.userdata.blocks = geometry_msg.PoseArray()
sm.userdata.pickup_pose = geometry_msg.Pose()
sm.userdata.place_pose = geometry_msg.Pose()
sm.userdata.topic = ''
smach.StateMachine.add(
'BlockDetection',
smach_ros.SimpleActionState(
'block_detection',
BlockDetectionAction,
goal_slots=['frame', 'table_height', 'block_size'],
result_slots=['blocks'],
server_wait_timeout=rospy.Duration(120)),
remapping={
'frame': 'frame',
'table_height': 'table_height',
'block_size': 'block_size',
'blocks': 'blocks'
},
transitions={
'succeeded': 'DragAndDrop',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('DragAndDrop',
smach_ros.SimpleActionState(
'interactive_manipulation',
InteractiveBlockManipulationAction,
goal_slots=['frame', 'block_size'],
result_slots=['pickup_pose', 'place_pose'],
server_wait_timeout=rospy.Duration(120)),
remapping={
'frame': 'frame',
'block_size': 'block_size',
'pickup_pose': 'pickup_pose',
'place_pose': 'place_pose'
},
transitions={
'succeeded': 'PickAndPlace',
'aborted': 'BlockDetection',
'preempted': 'preempted'
})
smach.StateMachine.add('PickAndPlace',
smach_ros.SimpleActionState(
'pick_and_place',
PickAndPlaceAction,
goal_slots=[
'frame', 'z_up', 'gripper_open',
'gripper_closed', 'pickup_pose',
'place_pose', 'topic'
],
result_slots=[],
server_wait_timeout=rospy.Duration(120)),
remapping={
'frame': 'frame',
'z_up': 'z_up',
'gripper_open': 'gripper_open',
'gripper_closed': 'gripper_closed',
'pickup_pose': 'pickup_pose',
'place_pose': 'place_pose'
},
transitions={
'succeeded': 'BlockDetection',
'aborted': 'BlockDetection',
'preempted': 'preempted'
})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
rospy.signal_shutdown('All done.')
def get_body_pose_array_msg(self, all_poses):
"""Given all segment poses, extract the body segment poses and TCP poses from that.
The body poses are simply all poses without hand segment poses and property poses.
:param all_poses: PoseArray A pose array composed of all segment poses.
:return: PoseArray PoseStamped PoseStamped PoseStamped PoseStamped
Body segment poses, left hand pose, right hand pose, left sole pose, right sole pose.
"""
assert isinstance(all_poses, GeometryMsg.PoseArray)
body_pose_array_msg = GeometryMsg.PoseArray()
body_pose_array_msg.header = all_poses.header
left_tcp_msg = GeometryMsg.PoseStamped()
right_tcp_msg = GeometryMsg.PoseStamped()
left_sole_msg = GeometryMsg.PoseStamped()
right_sole_msg = GeometryMsg.PoseStamped()
left_shoulder_msg = GeometryMsg.PoseStamped()
right_shoulder_msg = GeometryMsg.PoseStamped()
left_upper_arm_msg = GeometryMsg.PoseStamped()
right_upper_arm_msg = GeometryMsg.PoseStamped()
left_forearm_msg = GeometryMsg.PoseStamped()
right_forearm_msg = GeometryMsg.PoseStamped()
# Initialize header
left_tcp_msg.header = all_poses.header
right_tcp_msg.header = left_tcp_msg.header
left_sole_msg.header = left_tcp_msg.header
right_sole_msg.header = left_tcp_msg.header
left_shoulder_msg.header = left_tcp_msg.header
right_shoulder_msg.header = left_tcp_msg.header
left_upper_arm_msg.header = left_tcp_msg.header
right_upper_arm_msg.header = left_tcp_msg.header
left_forearm_msg.header = left_tcp_msg.header
right_forearm_msg.header = left_tcp_msg.header
# all_poses should at least contain body segment poses
segment_id = 0
for p in all_poses.poses:
body_pose_array_msg.poses.append(p)
if segment_id == 7:
right_shoulder_msg.pose = p
if segment_id == 8:
right_upper_arm_msg.pose = p
if segment_id == 9:
right_forearm_msg.pose = p
if segment_id == 10:
right_tcp_msg.pose = p
if segment_id == 11:
left_shoulder_msg.pose = p
if segment_id == 12:
left_upper_arm_msg.pose = p
if segment_id == 13:
left_forearm_msg.pose = p
if segment_id == 14:
left_tcp_msg.pose = p
if segment_id == 18:
right_sole_msg.pose = p
if segment_id == 22:
left_sole_msg.pose = p
segment_id += 1
if segment_id == self.header.body_segments_num:
break
assert len(body_pose_array_msg.poses) == self.header.body_segments_num
return [
body_pose_array_msg, left_tcp_msg, right_tcp_msg, left_sole_msg,
right_sole_msg, left_shoulder_msg, left_upper_arm_msg,
left_forearm_msg, right_shoulder_msg, right_upper_arm_msg,
right_forearm_msg
]
prev_y = y
prev_x = x
if (start == 0):
start = 1
def latLonToTileCoords(lat, lon):
rho = np.pi / 180
lat_rad = lat * rho
n = 2**14
x = n * ((lon + 180) / 360.0)
y = n * (1 - (np.log(np.tan(lat_rad) + (1 / np.cos(lat_rad))) / np.pi)) / 2
return x, y
if __name__ == '__main__':
signal.signal(signal.SIGINT, sigint_handler)
prev_x = 0
prev_y = 0
path = gm.PoseArray()
path.header.frame_id = 'base_link'
start = 0
rospy.init_node('latlon_to_xy', anonymous=True)
rospy.Subscriber("LatLon", Float64MultiArray, callback)
pub = rospy.Publisher("tiles", gm.PoseArray, queue_size=10)
rate = rospy.Rate(1) # 10hz
while True:
if (start != 0):
pub.publish(path)
rate.sleep()