def allObjectsInsideBin(scene, bin_num): global _tflistener for i in range(len(scene.objects)): poselist_shelf = poseTransform(pose2list(scene.objects[i].pose), 'map', 'shelf', _tflistener) if not inside_bin(poselist_shelf[0:3], bin_num): return False return True
def allObjectsInsideBin(scene, bin_num): global _tflistener for i in range(len(scene.objects)): poselist_shelf = poseTransform(pose2list(scene.objects[i].pose), 'map', 'shelf', _tflistener) if not inside_bin(poselist_shelf[0:3], bin_num): return False return True
def transformObjectsFromCapsenToDesiredFrame(scene, scene_frame_id): global _tflistener global br newscene = copy.deepcopy(scene) for i in range(len(scene.objects)): poselist_capsen_world = poseTransform(pose2list(scene.objects[i].pose), scene_frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf(scene.objects[i].name) # x,y,z,qx,qy,qz,qw poselist_world = transformBack(cap_T_our, poselist_capsen_world) newscene.objects[i].pose = poselist2pose(poselist_world) #pubFrame(br, poselist_world, 'obj_%s' % scene.objects[i].name, 'map') return newscene
def transformObjectsFromCapsenToDesiredFrame(scene, scene_frame_id): global _tflistener global br newscene = copy.deepcopy(scene) for i in range(len(scene.objects)): poselist_capsen_world = poseTransform(pose2list(scene.objects[i].pose), scene_frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf( scene.objects[i].name) # x,y,z,qx,qy,qz,qw poselist_world = transformBack(cap_T_our, poselist_capsen_world) newscene.objects[i].pose = poselist2pose(poselist_world) #pubFrame(br, poselist_world, 'obj_%s' % scene.objects[i].name, 'map') return newscene
def main(argv=None): if argv is None: argv = sys.argv rospy.init_node('calib', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(1) calibtag = 'link6tag1' if len(argv) == 2: calibtag = argv[1] # kinect calib joints # j1: 9.93 # j2: 39.46 # j3: -32.74 # j4: 69.54 # j5: -11.26 # j6: -70.88 #link6tag1 #link6tag3 #link6tag1 #link6tag3 # j1: -16.98 # j2: 65.55 # j3: -13.48 # j4: -21.15 # j5: -51.26 # j6: 10.63 #link6tag1 #link6tag3 #filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/camera_extrinsic_calib_data/camera_extrinsic_calib_data.json' if calibtag == 'link2tag': tag_link_transform = [0, 0.7, -0.2145-0.005, -math.pi, 0, 0] link_frame_id = 'link_2' elif calibtag == 'linkbasetag': print 'move to q =', (np.array([2.35,-67.73,62.39,-154.9,-30.11,11]) / 180 * math.pi).tolist() print 'move to q =', (np.array([2.35,-77.73,72.39,-154.9,-20.11,11]) / 180 * math.pi).tolist() tag_link_transform = [0.5291, 0.4428, 0.005, math.pi, 0, 0] link_frame_id = 'base_link' elif calibtag == 'link6tag1': #print 'move to q =', (np.array([9.93,39.37,-30.63,60.64,-12.12,-61.79]) / 180 * math.pi).tolist() tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi/2] link_frame_id = 'link_6' elif calibtag == 'link6tag2': tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, 0] link_frame_id = 'link_6' elif calibtag == 'link6tag3': tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, -math.pi/2] link_frame_id = 'link_6' elif calibtag == 'link6tag4': tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi] link_frame_id = 'link_6' # visualize it pubFrame(br, pose=tag_link_transform, frame_id='tag', parent_frame_id=link_frame_id, npub=10) tag_map_transform = poseTransform(tag_link_transform, link_frame_id, 'map', listener) apriltag_topic = 'tag_detections' while True: tagdetect = ROS_Wait_For_Msg(apriltag_topic, AprilTagDetectionArray).getmsg() if len(tagdetect.detections) == 1: tag_kinect_transform = pose2list(tagdetect.detections[0].pose.pose) break tag_kinect_tfm_mat = matrix_from_xyzquat(translate=tag_kinect_transform[0:3], quaternion=tag_kinect_transform[3:7]) tag_map_tfm_mat = matrix_from_xyzquat(translate=tag_map_transform[0:3], quaternion=tag_map_transform[3:7]) kinect_map_tfm_mat = np.dot(tag_map_tfm_mat, np.linalg.inv(tag_kinect_tfm_mat)) kinect_map_pose = tfm.translation_from_matrix(kinect_map_tfm_mat).tolist() + tfm.quaternion_from_matrix(kinect_map_tfm_mat).tolist() link6tag = ['link6tag1', 'link6tag2', 'link6tag3', 'link6tag4'] if calibtag in link6tag: print kinect_map_pose pubFrame(br, pose=kinect_map_pose, frame_id='new_kinect_pose', parent_frame_id='map', npub=10) else: realsense_link5_pose = poseTransform(kinect_map_pose, link_frame_id, 'link_5', listener) pubFrame(br, pose=realsense_link5_pose, frame_id='new_realsense_pose', parent_frame_id='link_5', npub=10) print realsense_link5_pose
def main(argv=None): if argv is None: argv = sys.argv parser = optparse.OptionParser() #import socket #useObject = not (socket.gethostname() == 'mcube-002' or socket.gethostname() == 'mcube-003') parser.add_option('-o', '--obj', action="store_true", dest='useObject', help='To use manual object fitting', default=False) parser.add_option('-c', '--cammarker', action="store_true", dest='useCamMarker', help='To use manual camera fitting or not', default=False) parser.add_option('-s', '--shelfmarker', action="store_true", dest='useShelfMarker', help='To use manual shelf fitting or not', default=False) parser.add_option('-a', '--all', action="store_true", dest='useAllMarker', help='To use manual fitting or not', default=False) rospy.init_node("manual_fit_server") global currentExpoPose global currentCrayolaPose global currentKinectPose global currentShelfPose global currentJoint_realsense global currentJoint_realsense_link5 global brKinect global brShelf global nkinect global vis_pub global opt global pyramid_marker (opt, args) = parser.parse_args() currentExpoPose = Pose() currentCrayolaPose = Pose() currentKinectPose = [Pose(),Pose()] currentShelfPose = Pose() currentJoint_realsense = Pose() brKinect = TransformBroadcaster() brShelf = TransformBroadcaster() global listener listener = tf.TransformListener() rospy.sleep(1) # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("affordance_marker") # on mcube-002 don't use object if opt.useObject or opt.useAllMarker: # create an interactive marker for Expo pose = [1.58220779896, 0.296458333731, 1.12021064758, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023] int_marker = createInteractiveMarker('ExpoEraser', *pose) meshmarker = createMeshMarker('package://apc_config/models/object_meshes/expo_dry_erase_board_eraser.stl', offset=tuple(pose[0:3]), rgba=(0,1,0,1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Crayola pose = [1.63922035694, -0.273455768824, 1.12480783463, -0.0384246744215, 0.697018921375, -0.715074837208, 0.0368278548121] int_marker = createInteractiveMarker('Crayola', *pose) meshmarker = createMeshMarker('package://apc_config/models/object_meshes/crayola_64_ct.stl', offset=tuple(pose[0:3]), rgba=(1,0,0,1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Shelf currentShelfPose.position.x = rospy.get_param('shelf_pose/x') currentShelfPose.position.y = rospy.get_param('shelf_pose/y') currentShelfPose.position.z = rospy.get_param('shelf_pose/z') currentShelfPose.orientation.x = rospy.get_param('shelf_pose/qx') currentShelfPose.orientation.y = rospy.get_param('shelf_pose/qy') currentShelfPose.orientation.z = rospy.get_param('shelf_pose/qz') currentShelfPose.orientation.w = rospy.get_param('shelf_pose/qw') int_marker = createInteractiveMarker('Shelf', currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z, currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w) meshmarker = createMeshMarker('package://apc_config/models/kiva_pod/meshes/pod_lowres.stl', offset=(currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z), rgba=(0,0,1,0.5), orientation=(currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w)) int_marker.controls.append(meshmarker) if opt.useShelfMarker or opt.useAllMarker: int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) rospy.Timer(rospy.Duration(0.01), frameCallbackShelf) # create an interactive marker for Kinects nkinect = 2 # old manual fit kinect pose #pose = [[0.305660784245, -0.883868515491, 1.66722476482, -0.244159132242, 0.827043175697, 0.0950953885913, 0.497335940599], # [0.367319256067, 0.855090618134, 1.78848266602, 0.800817668438, -0.247087046504, 0.528468191624, 0.135500892997]] #pose = [[0.2643078824103202, -0.8475994497881857, 1.5983246933067445, -0.23088847539406132, 0.822983396811164, 0.08995383249481428, 0.511169994763495], #[0.3659496377911322, 0.8087564208783671, 1.779781933883937, 0.8065800933146582, -0.24219685501387836, 0.5258798075015827, 0.11924245508274978]] # pose = [[0.2615747933866079, -0.847265732303331, 1.5924661390467871, -0.22946772410305405, 0.8209085501266424, 0.08173898334984725, 0.5164978476596324], # [0.35734815800402436, 0.809898201970986, 1.7640327819976598, 0.8036871694243676, -0.2417794937981231, 0.5299934028148222, 0.12138698747903716]] pose = [[0.254395043470156, -0.849313121283084, 1.5914400427256015, -0.2499411509851439, 0.8071675924380618, 0.11813937247844475, 0.5215836382491776], [0.3581305540948658, 0.8062610720617674, 1.7711358584054768, 0.8090512113331657, -0.2868798404832606, 0.494998046288383, 0.13458167753046513]] for i in range(nkinect): currentKinectPose[i] = poselist2pose( pose[i] ) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('Kinect%d' % (i+1), *pose[i]) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) #pose = [-0.0246461518109, -0.000698581337929, 0.0109369456768, 0.706081748009, 0.706393122673, -0.0283508431166, 0.04066388309] #pose = [-0.030973277986, 0.0219592899084, -0.0248876661062, 0.702517032623, 0.711110830307, 0.0224311985075, 0.0169796515256] #pose = [-0.0141396149993, 0.0219592899084, -0.0248876661062, 0.70285320282, 0.71131336689, 0.00553341582417, -0.000123182311654] #pose = [-0.0186747914648, 0.00628428290067, -0.0175085197216, 0.702853216106, 0.711313378682, 0.00553342037479, -0.0001231898954] #pose = [-0.0191506993199, 0.0277181266145, -0.0117737966167, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927] pose = [-0.023043141477, 0.0275422775115, -0.0114395739518, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927] pose_world = poseTransform(pose, 'link_5', 'map', listener) currentJoint_realsense = poselist2pose( pose_world ) currentJoint_realsense_link5 = poselist2pose( pose ) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('joint_realsense', *pose_world, frame_id='map') # cubemarker = createCubeMarker(offset=tuple(pose[0:3]), rgba=(1,0,1,0.5), # orientation=tuple(pose[3:7]), # scale=(0.01,0.01,0.01)) #int_marker.controls.append(cubemarker) viewMarker = createViewMarker(frame_id='realsense_depth_optical_frame') #points = [[0.15748027363422223, 0.08267716536239379, 0.2], [-0.15748027363422223, 0.08267716536239379, 0.2], [0.15748027363422223, -0.08267716536239379, 0.2], [-0.15748027363422223, -0.08267716536239379, 0.2] ] # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(1,1,0,0.1), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.001) # int_marker.controls.append(meshmarker) # # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(0,1,0,0.5), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.002) # int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) pyramid_marker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', offset=tuple([0,0,0]), rgba=(1,1,0,0.1), orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.002, scales = [2, 2,1]).markers[0] rospy.Timer(rospy.Duration(0.01), frameCallbackKinect) server.applyChanges() # ros services startPoseService() rospy.spin()
def main(argv=None): if argv is None: argv = sys.argv rospy.init_node('calib', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(1) calibtag = 'link6tag1' if len(argv) == 2: calibtag = argv[1] #filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/camera_extrinsic_calib_data/camera_extrinsic_calib_data.json' if calibtag == 'link2tag': tag_link_transform = [0, 0.7, -0.2145-0.005, -math.pi, 0, 0] link_frame_id = 'link_2' elif calibtag == 'realsense1': print 'move to q =', (np.array([ 0.0, -23.02, 34.73, 154.93, -25.15, 0.0]) / 180 * math.pi).tolist() r = 0.1923/2 tag_link_transform = [0.4033-0.230 -r, -1.121/2 + 0.044 + r, 0.005, 0, 0, 0] link_frame_id = 'base_link' elif calibtag == 'link6tag1': #print 'move to q =', (np.array([9.93,39.37,-30.63,60.64,-12.12,-61.79]) / 180 * math.pi).tolist() tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi/2] link_frame_id = 'link_6' elif calibtag == 'link6tag2': tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, 0] link_frame_id = 'link_6' elif calibtag == 'link6tag3': tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, -math.pi/2] link_frame_id = 'link_6' elif calibtag == 'link6tag4': tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi] link_frame_id = 'link_6' # visualize it pubFrame(br, pose=tag_link_transform, frame_id='tag', parent_frame_id=link_frame_id, npub=10) tag_map_transform = poseTransform(tag_link_transform, link_frame_id, 'map', listener) apriltag_topic = '/pr_apriltags/detections' print 'Wait for apriltag detection' tag_camera_transforms = [] for i in xrange(5): while True: tagdetect = ROS_Wait_For_Msg(apriltag_topic, AprilTagDetections).getmsg() if len(tagdetect.detections) == 1: tag_camera_transform = pose2list(tagdetect.detections[0].pose) tag_camera_transforms.append(tag_camera_transform) break tag_camera_transform = np.average(np.array(tag_camera_transforms), axis = 0) tag_camera_tfm_mat = matrix_from_xyzquat(tag_camera_transform) tag_map_tfm_mat = matrix_from_xyzquat(tag_map_transform) camera_map_tfm_mat = np.dot(tag_map_tfm_mat, np.linalg.inv(tag_camera_tfm_mat)) camera_map_pose = tfm.translation_from_matrix(camera_map_tfm_mat).tolist() + tfm.quaternion_from_matrix(camera_map_tfm_mat).tolist() link6tag = ['link6tag1', 'link6tag2', 'link6tag3', 'link6tag4'] if calibtag in link6tag: print camera_map_pose pubFrame(br, pose=camera_map_pose, frame_id='new_kinect_pose', parent_frame_id='map', npub=10) elif calibtag == 'realsense1': realsense_link5_pose = poseTransform(camera_map_pose, link_frame_id, 'link_5', listener) pubFrame(br, pose=realsense_link5_pose, frame_id='new_realsense_pose', parent_frame_id='link_5', npub=10) print realsense_link5_pose
def _detectOneObject(obj_id, bin_num, kinect_num): global _detect_one_object_srv global br print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num # filter the point cloud pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num, kinect_num) if pc is None: return (None, None) # string[] model_names # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names' # ObjectConstraint[] constraints # These apply to all objects # --- # SceneHypothesis[] detections # 2. prepare constraints bin_cnstr = get_bin_cnstr()[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top ccnstr = [] tol = 0.9 # larger is more strict (trans,rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener) # 2.1 right ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([1,0,0], [bin_cnstr[0],0,0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.2 left ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([-1,0,0], [bin_cnstr[1],0,0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.3 back ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,1,0], [0,bin_cnstr[2],0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.4 front ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,-1,0], [0,bin_cnstr[3],0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.5 floor ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,1], [0,0,bin_cnstr[4]], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.6 top ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,-1], [0,0,bin_cnstr[5]], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.7 on floor floor_thick = 0.03 ccnstr.append( createCapsenConstraint(ObjectConstraint.SUPPORTING_PLANE, transformPlane([0,0,1], [0,0, bin_cnstr[4]-floor_thick/2], trans,rot, pc.header.frame_id), tol, bin_num) ) # string model_name # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # ObjectConstraint[] constraints # geometry_msgs/Pose true_pose # for testing # --- # # ObjectHypothesis[] detections with Timer('detect_one_object'): # detect using capsen service_name = '/detection_service/detect_one_object' req = DetectOneObjectRequest() req.model_name = obj_id req.cloud = pc req.constraints = ccnstr req.foreground_mask = foreground_mask #req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] print 'Waiting for service up: ', service_name rospy.wait_for_service(service_name) try: print 'Calling service:', service_name ret = _detect_one_object_srv(req) # ret.detections is a list of capsen_vision/ObjectHypothesis # string name # geometry_msgs/Pose pose # float32 score # float32[] score_components if len(ret.detections)>0: print len(ret.detections), 'ObjectHypothesis returned, max score', ret.detections[0].score for i in range(len(ret.detections)): poselist_capsen_world = poseTransform(pose2list(ret.detections[i].pose), pc.header.frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw poselist_world = transformBack(cap_T_our, poselist_capsen_world) # transform to our desired pose # check whether inside bin poselist_shelf = poseTransform(poselist_world, 'map', 'shelf', _tflistener) if inside_bin(poselist_shelf[0:3], bin_num): #pubFrame(br, poselist_world, 'obj', 'map') return (poselist_world, ret.detections[i].score) else: print 'reject hypo', i, 'because it is outside the target bin' print 'No ObjectHypothesis satisfy hard bin constraint' return (None, None) else: print 'No ObjectHypothesis returned' return (None, None) except: print 'Calling service:', service_name, 'failed' print 'encounters errors:', traceback.format_exc() return (None, None)
def _detectOneObject(obj_id, bin_num, kinect_num): global _detect_one_object_srv global br print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num # filter the point cloud pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num, kinect_num) if pc is None: return (None, None) # string[] model_names # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names' # ObjectConstraint[] constraints # These apply to all objects # --- # SceneHypothesis[] detections # 2. prepare constraints bin_cnstr = get_bin_cnstr( )[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top ccnstr = [] tol = 0.9 # larger is more strict (trans, rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener) # 2.1 right ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([1, 0, 0], [bin_cnstr[0], 0, 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.2 left ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([-1, 0, 0], [bin_cnstr[1], 0, 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.3 back ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 1, 0], [0, bin_cnstr[2], 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.4 front ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, -1, 0], [0, bin_cnstr[3], 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.5 floor ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 0, 1], [0, 0, bin_cnstr[4]], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.6 top ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 0, -1], [0, 0, bin_cnstr[5]], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.7 on floor floor_thick = 0.03 ccnstr.append( createCapsenConstraint( ObjectConstraint.SUPPORTING_PLANE, transformPlane([0, 0, 1], [0, 0, bin_cnstr[4] - floor_thick / 2], trans, rot, pc.header.frame_id), tol, bin_num)) # string model_name # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # ObjectConstraint[] constraints # geometry_msgs/Pose true_pose # for testing # --- # # ObjectHypothesis[] detections with Timer('detect_one_object'): # detect using capsen service_name = '/detection_service/detect_one_object' req = DetectOneObjectRequest() req.model_name = obj_id req.cloud = pc req.constraints = ccnstr req.foreground_mask = foreground_mask #req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] print 'Waiting for service up: ', service_name rospy.wait_for_service(service_name) try: print 'Calling service:', service_name ret = _detect_one_object_srv(req) # ret.detections is a list of capsen_vision/ObjectHypothesis # string name # geometry_msgs/Pose pose # float32 score # float32[] score_components if len(ret.detections) > 0: print len( ret.detections ), 'ObjectHypothesis returned, max score', ret.detections[ 0].score for i in range(len(ret.detections)): poselist_capsen_world = poseTransform( pose2list(ret.detections[i].pose), pc.header.frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw poselist_world = transformBack( cap_T_our, poselist_capsen_world) # transform to our desired pose # check whether inside bin poselist_shelf = poseTransform(poselist_world, 'map', 'shelf', _tflistener) if inside_bin(poselist_shelf[0:3], bin_num): #pubFrame(br, poselist_world, 'obj', 'map') return (poselist_world, ret.detections[i].score) else: print 'reject hypo', i, 'because it is outside the target bin' print 'No ObjectHypothesis satisfy hard bin constraint' return (None, None) else: print 'No ObjectHypothesis returned' return (None, None) except: print 'Calling service:', service_name, 'failed' print 'encounters errors:', traceback.format_exc() return (None, None)
def main(argv=None): if argv is None: argv = sys.argv parser = optparse.OptionParser() #import socket #useObject = not (socket.gethostname() == 'mcube-002' or socket.gethostname() == 'mcube-003') parser.add_option('-o', '--obj', action="store_true", dest='useObject', help='To use manual object fitting', default=False) parser.add_option('-c', '--cammarker', action="store_true", dest='useCamMarker', help='To use manual camera fitting or not', default=False) parser.add_option('-s', '--shelfmarker', action="store_true", dest='useShelfMarker', help='To use manual shelf fitting or not', default=False) parser.add_option('-a', '--all', action="store_true", dest='useAllMarker', help='To use manual fitting or not', default=False) rospy.init_node("manual_fit_server") global currentExpoPose global currentCrayolaPose global currentKinectPose global currentShelfPose global currentJoint_realsense global currentJoint_realsense_link5 global brKinect global brShelf global nkinect global vis_pub global opt global pyramid_marker (opt, args) = parser.parse_args() currentExpoPose = Pose() currentCrayolaPose = Pose() currentKinectPose = [Pose(), Pose()] currentShelfPose = Pose() currentJoint_realsense = Pose() brKinect = TransformBroadcaster() brShelf = TransformBroadcaster() global listener listener = tf.TransformListener() rospy.sleep(1) # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("affordance_marker") # on mcube-002 don't use object if opt.useObject or opt.useAllMarker: # create an interactive marker for Expo pose = [ 1.58220779896, 0.296458333731, 1.12021064758, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023 ] int_marker = createInteractiveMarker('ExpoEraser', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/object_meshes/expo_dry_erase_board_eraser.stl', offset=tuple(pose[0:3]), rgba=(0, 1, 0, 1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Crayola pose = [ 1.63922035694, -0.273455768824, 1.12480783463, -0.0384246744215, 0.697018921375, -0.715074837208, 0.0368278548121 ] int_marker = createInteractiveMarker('Crayola', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/object_meshes/crayola_64_ct.stl', offset=tuple(pose[0:3]), rgba=(1, 0, 0, 1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Shelf currentShelfPose.position.x = rospy.get_param('shelf_pose/x') currentShelfPose.position.y = rospy.get_param('shelf_pose/y') currentShelfPose.position.z = rospy.get_param('shelf_pose/z') currentShelfPose.orientation.x = rospy.get_param('shelf_pose/qx') currentShelfPose.orientation.y = rospy.get_param('shelf_pose/qy') currentShelfPose.orientation.z = rospy.get_param('shelf_pose/qz') currentShelfPose.orientation.w = rospy.get_param('shelf_pose/qw') int_marker = createInteractiveMarker( 'Shelf', currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z, currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w) meshmarker = createMeshMarker( 'package://apc_config/models/kiva_pod/meshes/pod_lowres.stl', offset=(currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z), rgba=(0, 0, 1, 0.5), orientation=(currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w)) int_marker.controls.append(meshmarker) if opt.useShelfMarker or opt.useAllMarker: int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) rospy.Timer(rospy.Duration(0.01), frameCallbackShelf) # create an interactive marker for Kinects nkinect = 2 # old manual fit kinect pose #pose = [[0.305660784245, -0.883868515491, 1.66722476482, -0.244159132242, 0.827043175697, 0.0950953885913, 0.497335940599], # [0.367319256067, 0.855090618134, 1.78848266602, 0.800817668438, -0.247087046504, 0.528468191624, 0.135500892997]] #pose = [[0.2643078824103202, -0.8475994497881857, 1.5983246933067445, -0.23088847539406132, 0.822983396811164, 0.08995383249481428, 0.511169994763495], #[0.3659496377911322, 0.8087564208783671, 1.779781933883937, 0.8065800933146582, -0.24219685501387836, 0.5258798075015827, 0.11924245508274978]] # pose = [[0.2615747933866079, -0.847265732303331, 1.5924661390467871, -0.22946772410305405, 0.8209085501266424, 0.08173898334984725, 0.5164978476596324], # [0.35734815800402436, 0.809898201970986, 1.7640327819976598, 0.8036871694243676, -0.2417794937981231, 0.5299934028148222, 0.12138698747903716]] pose = [[ 0.254395043470156, -0.849313121283084, 1.5914400427256015, -0.2499411509851439, 0.8071675924380618, 0.11813937247844475, 0.5215836382491776 ], [ 0.3581305540948658, 0.8062610720617674, 1.7711358584054768, 0.8090512113331657, -0.2868798404832606, 0.494998046288383, 0.13458167753046513 ]] for i in range(nkinect): currentKinectPose[i] = poselist2pose(pose[i]) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('Kinect%d' % (i + 1), *pose[i]) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) #pose = [-0.0246461518109, -0.000698581337929, 0.0109369456768, 0.706081748009, 0.706393122673, -0.0283508431166, 0.04066388309] #pose = [-0.030973277986, 0.0219592899084, -0.0248876661062, 0.702517032623, 0.711110830307, 0.0224311985075, 0.0169796515256] #pose = [-0.0141396149993, 0.0219592899084, -0.0248876661062, 0.70285320282, 0.71131336689, 0.00553341582417, -0.000123182311654] #pose = [-0.0186747914648, 0.00628428290067, -0.0175085197216, 0.702853216106, 0.711313378682, 0.00553342037479, -0.0001231898954] #pose = [-0.0191506993199, 0.0277181266145, -0.0117737966167, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927] pose = [ -0.023043141477, 0.0275422775115, -0.0114395739518, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927 ] pose_world = poseTransform(pose, 'link_5', 'map', listener) currentJoint_realsense = poselist2pose(pose_world) currentJoint_realsense_link5 = poselist2pose(pose) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('joint_realsense', *pose_world, frame_id='map') # cubemarker = createCubeMarker(offset=tuple(pose[0:3]), rgba=(1,0,1,0.5), # orientation=tuple(pose[3:7]), # scale=(0.01,0.01,0.01)) #int_marker.controls.append(cubemarker) viewMarker = createViewMarker(frame_id='realsense_depth_optical_frame') #points = [[0.15748027363422223, 0.08267716536239379, 0.2], [-0.15748027363422223, 0.08267716536239379, 0.2], [0.15748027363422223, -0.08267716536239379, 0.2], [-0.15748027363422223, -0.08267716536239379, 0.2] ] # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(1,1,0,0.1), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.001) # int_marker.controls.append(meshmarker) # # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(0,1,0,0.5), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.002) # int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) pyramid_marker = createMeshMarker( 'package://apc_config/models/object_meshes/pyramid.stl', offset=tuple([0, 0, 0]), rgba=(1, 1, 0, 0.1), orientation=tuple([0, 0, 0, 1]), frame_id='realsense_depth_optical_frame', scale=0.002, scales=[2, 2, 1]).markers[0] rospy.Timer(rospy.Duration(0.01), frameCallbackKinect) server.applyChanges() # ros services startPoseService() rospy.spin()
def main(argv=None): if argv is None: argv = sys.argv rospy.init_node('calib', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(1) calibtag = 'link6tag1' if len(argv) == 2: calibtag = argv[1] # kinect calib joints # j1: 9.93 # j2: 39.46 # j3: -32.74 # j4: 69.54 # j5: -11.26 # j6: -70.88 #link6tag1 #link6tag3 #link6tag1 #link6tag3 # j1: -16.98 # j2: 65.55 # j3: -13.48 # j4: -21.15 # j5: -51.26 # j6: 10.63 #link6tag1 #link6tag3 #filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/camera_extrinsic_calib_data/camera_extrinsic_calib_data.json' if calibtag == 'link2tag': tag_link_transform = [0, 0.7, -0.2145 - 0.005, -math.pi, 0, 0] link_frame_id = 'link_2' elif calibtag == 'linkbasetag': print 'move to q =', ( np.array([2.35, -67.73, 62.39, -154.9, -30.11, 11]) / 180 * math.pi).tolist() print 'move to q =', ( np.array([2.35, -77.73, 72.39, -154.9, -20.11, 11]) / 180 * math.pi).tolist() tag_link_transform = [0.5291, 0.4428, 0.005, math.pi, 0, 0] link_frame_id = 'base_link' elif calibtag == 'link6tag1': #print 'move to q =', (np.array([9.93,39.37,-30.63,60.64,-12.12,-61.79]) / 180 * math.pi).tolist() tag_link_transform = [ -(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi / 2 ] link_frame_id = 'link_6' elif calibtag == 'link6tag2': tag_link_transform = [ -(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, 0 ] link_frame_id = 'link_6' elif calibtag == 'link6tag3': tag_link_transform = [ -(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, -math.pi / 2 ] link_frame_id = 'link_6' elif calibtag == 'link6tag4': tag_link_transform = [ -(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi ] link_frame_id = 'link_6' # visualize it pubFrame(br, pose=tag_link_transform, frame_id='tag', parent_frame_id=link_frame_id, npub=10) tag_map_transform = poseTransform(tag_link_transform, link_frame_id, 'map', listener) apriltag_topic = 'tag_detections' while True: tagdetect = ROS_Wait_For_Msg(apriltag_topic, AprilTagDetectionArray).getmsg() if len(tagdetect.detections) == 1: tag_kinect_transform = pose2list(tagdetect.detections[0].pose.pose) break tag_kinect_tfm_mat = matrix_from_xyzquat( translate=tag_kinect_transform[0:3], quaternion=tag_kinect_transform[3:7]) tag_map_tfm_mat = matrix_from_xyzquat(translate=tag_map_transform[0:3], quaternion=tag_map_transform[3:7]) kinect_map_tfm_mat = np.dot(tag_map_tfm_mat, np.linalg.inv(tag_kinect_tfm_mat)) kinect_map_pose = tfm.translation_from_matrix(kinect_map_tfm_mat).tolist( ) + tfm.quaternion_from_matrix(kinect_map_tfm_mat).tolist() link6tag = ['link6tag1', 'link6tag2', 'link6tag3', 'link6tag4'] if calibtag in link6tag: print kinect_map_pose pubFrame(br, pose=kinect_map_pose, frame_id='new_kinect_pose', parent_frame_id='map', npub=10) else: realsense_link5_pose = poseTransform(kinect_map_pose, link_frame_id, 'link_5', listener) pubFrame(br, pose=realsense_link5_pose, frame_id='new_realsense_pose', parent_frame_id='link_5', npub=10) print realsense_link5_pose