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()