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 currentWorldcamPose
global currentShelfPose
global currentTotePose
global currentBin0Pose
global currentBin1Pose
global currentBin2Pose
global currentBin3Pose
global currentRailsPose
global currentBinInsideCollisionPose
global currentBinOutsideCollisionPose
global br, brWorldcam
global nworldcam
global vis_pub
global opt
global pyramid_marker
(opt, args) = parser.parse_args()
currentExpoPose = Pose()
currentCrayolaPose = Pose()
currentWorldcamPose = [Pose()]
currentShelfPose = Pose()
currentTotePose = Pose()
currentRailsPose = Pose()
currentBin0Pose = Pose()
currentBin1Pose = Pose()
currentBin2Pose = Pose()
currentBin3Pose = Pose()
currentBinInsideCollisionPose = Pose()
currentBinOutsideCollisionPose = Pose()
currentJoint_realsense = Pose()
br = TransformBroadcaster()
brWorldcam = 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.01179456711, -0.596704602242, -0.126571118832, 0.0,
-0.707106769085, 0.0, 0.707106769085
]
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.01179456711, 0.588456273079, 0.0363664329052, 0.0,
-0.707106769085, 0.0, 0.707106769085
]
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)
#
# storage system
pose = []
for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']:
print 'rospy.get_param(tote_pose/ + term)', rospy.get_param(
'tote_pose/' + term)
pose.append(rospy.get_param('tote_pose/' + term))
currentTotePose.position.x = pose[0]
currentTotePose.position.y = pose[1]
currentTotePose.position.z = pose[2]
currentTotePose.orientation.x = pose[3]
currentTotePose.orientation.y = pose[4]
currentTotePose.orientation.z = pose[5]
currentTotePose.orientation.w = pose[6]
int_marker = createInteractiveMarker('tote', *pose)
meshmarker = createMeshMarker(
'package://apc_config/models/tote/storage_system_compressed.stl',
offset=tuple(pose[0:3]),
rgba=(0.5, 0.5, 0.5, 1),
scale=0.001,
orientation=tuple(pose[3:7]),
frame_id='/map')
int_marker.controls.append(meshmarker)
#~ int_marker.controls.extend(createMoveControls())
server.insert(int_marker, processFeedback)
rospy.Timer(rospy.Duration(0.1), frameCallbackShelfTote)
# moving rails
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# print 'rospy.get_param(tote_pose/ + term)', term))
#rospy.get_param('tote_pose/' + term)
# pose.append(rospy.get_param('tote_pose/' +
# pose[1] = pose[1] + 1*(rospy.get_param('bin3_pose/y') - rospy.get_param('bin2_pose/y'))
# int_marker = createInteractiveMarker('moving_rails', *pose)
#
# meshmarker = createMeshMarker('package://apc_config/models/tote/moving_rails_compressed.stl',
# offset=tuple(pose[0:3]), rgba=(0.5,0.5,0.5,1), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# #~ int_marker.controls.extend(createMoveControls())
# server.insert(int_marker, processFeedback)
# rospy.Timer(rospy.Duration(0.1), frameCallbackShelfTote)
# bin0
pose = []
for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']:
pose.append(rospy.get_param('bin0_pose/' + term))
int_marker = createInteractiveMarker('bin0', *pose)
meshmarker = createMeshMarker(
'package://apc_config/models/bin/bin_large.stl',
offset=tuple(pose[0:3]),
rgba=(1, 0, 0, 1),
scale=0.001,
orientation=tuple(pose[3:7]),
frame_id='/map')
int_marker.controls.append(meshmarker)
#int_marker.controls.extend(createMoveControls())
server.insert(int_marker, processFeedback)
# bin1
pose = []
for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']:
pose.append(rospy.get_param('bin1_pose/' + term))
int_marker = createInteractiveMarker('bin1', *pose)
meshmarker = createMeshMarker(
'package://apc_config/models/bin/bin_large.stl',
offset=tuple(pose[0:3]),
rgba=(1, 0, 0, 1),
scale=0.001,
orientation=tuple(pose[3:7]),
frame_id='/map')
int_marker.controls.append(meshmarker)
#int_marker.controls.extend(createMoveControls())
server.insert(int_marker, processFeedback)
# create an interactive marker for left bin (tote)
# bin2
pose = []
for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']:
pose.append(rospy.get_param('bin2_pose/' + term))
int_marker = createInteractiveMarker('bin2', *pose)
meshmarker = createMeshMarker(
'package://apc_config/models/bin/bin_large.stl',
offset=tuple(pose[0:3]),
rgba=(1, 0, 0, 1),
scale=0.001,
orientation=tuple(pose[3:7]),
frame_id='/map')
int_marker.controls.append(meshmarker)
#int_marker.controls.extend(createMoveControls())
server.insert(int_marker, processFeedback)
#
# # bin3
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin3_pose/' + term))
# #~ pose[1] = rospy.get_param('bin2_pose/y')
# int_marker = createInteractiveMarker('bin3', *pose)
#
# meshmarker = createMeshMarker('package://apc_config/models/bin/bin_small.stl',
# offset=tuple(pose[0:3]), rgba=(1,0,0,1), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# #int_marker.controls.extend(createMoveControls())
# server.insert(int_marker, processFeedback)
###########
## boxes ##
###########
#~boxes 4-5
# if rospy.get_param('~is_picking', False):
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin4_pose/' + term))
# pose[2] = pose[2] - .07512*1
# int_marker = createInteractiveMarker('box_45', *pose)
# meshmarker = createMeshMarker('package://apc_config/models/boxes/box_45.stl',
# offset=tuple(pose[0:3]), rgba=(0.3,0.3,0.3,1), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
#
# #~box 4
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin4_pose/' + term))
# #~ pose[2] = pose[2] + .07512*0
# int_marker = createInteractiveMarker('cardboard_4', *pose)
#
# meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_4.stl',
# offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
#
# #~box 5
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin5_pose/' + term))
# #~ pose[2] = pose[2] + .1037*1
# int_marker = createInteractiveMarker('cardboard_5', *pose)
# meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_5.stl',
# offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
# else:
#
# #~boxes 6-7
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin6_pose/' + term))
# pose[2]-= rospy.get_param('/bin6/z_flap')
# int_marker = createInteractiveMarker('box_67', *pose)
# meshmarker = createMeshMarker('package://apc_config/models/boxes/box_67_compressed.stl',
# #~ offset=tuple(pose[0:3]), rgba=(0.1,0.1,0.1,1), scale = 0.001,
# offset=tuple(pose[0:3]), rgba=(0.3,0.3,0.3,1), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
#
# #~box 6
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin6_pose/' + term))
# #~ pose[2] = pose[2] + 0.07135
# #~ pose[1] = rospy.get_param('bin2_pose/y')
# int_marker = createInteractiveMarker('bin6_cardboard', *pose)
#
# meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_6.stl',
# offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
#
# #~box 7
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin7_pose/' + term))
# #~ pose[1] = rospy.get_param('bin2_pose/y')
# int_marker = createInteractiveMarker('cardboard_7', *pose)
#
# meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_7.stl',
# offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
#
# #~box 8
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin8_pose/' + term))
# pose[2]= pose[2] - rospy.get_param('/bin8/z_flap')
# int_marker = createInteractiveMarker('bin8', *pose)
#
# meshmarker = createMeshMarker('package://apc_config/models/boxes/box_8_compressed.stl',
# #~ offset=tuple(pose[0:3]), rgba=(0.5,0.5,0.5,1), scale = 0.001,
# offset=tuple(pose[0:3]), rgba=(0.3,0.3,0.3,1), scale = 0.001,
# #~ offset=tuple(pose[0:3]), rgba=(0.1,0.1,0.1,1), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
#
# #~box 8
# pose = []
# for term in ['x','y','z','qx','qy','qz','qw']:
# pose.append(rospy.get_param('bin8_pose/' + term))
# int_marker = createInteractiveMarker('bin8_cardboard', *pose)
#
# meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_8.stl',
# offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001,
# orientation=tuple(pose[3:7]), frame_id='/map')
# int_marker.controls.append(meshmarker)
# server.insert(int_marker, processFeedback)
#############
## Cameras ##
#############
pose = []
pose.append(
[1.3216, -.55862, .45089, 0.68986, .678841, -.185574, -.169784])
pose.append(
[0.54191, -0.54436, 0.28256, 0.65065, -0.63561, 0.29037, -0.29724])
pose.append(
[1.3225, -0.18962, 0.44935, 0.66217, 0.68597, -0.22759, -0.19795])
pose.append(
[0.48932, -0.18366, 0.038885, -0.42587, 0.42257, -0.56458, 0.56685])
pose.append(
[0.54073, 0.18461, 0.27804, 0.65751, -0.65352, 0.26679, -0.26348])
pose.append(
[1.3213, -0.55948, 0.37547, 0.58127, 0.60183, -0.40335, -0.37045])
pose.append(
[1.321, 0.57179, 0.36449, 0.60112, 0.58022, -0.38807, -0.38911])
pose.append(
[0.49027, 0.54894, 0.032927, -0.41015, 0.39011, -0.58548, 0.58035])
pose.append(
[0.48974, -0.55503, 0.042641, -0.42376, 0.42326, -0.567, 0.5655])
pose.append([1.3201, 0.19125, 0.36687, 0.6001, 0.59251, -0.3876, -0.37227])
pose.append(
[0.54189, -0.17822, 0.28174, 0.6454, -0.64361, 0.28747, -0.29425])
pose.append(
[1.3247, 0.19321, 0.44441, 0.68986, 0.67884, -0.18557, -0.16978])
pose.append(
[0.5406, 0.55541, 0.27902, 0.63946, -0.62909, 0.31036, -0.31467])
pose.append(
[1.3227, 0.57033, 0.44087, 0.68514, 0.67575, -0.1933, -0.19126])
pose.append(
[1.3206, -0.18846, 0.37101, 0.5894, 0.59423, -0.39252, -0.38134])
pose.append(
[0.48962, 0.17463, 0.035821, -0.41771, 0.40725, -0.57236, 0.57625])
#~camera 0
for i in range(0, 16):
print 'i', i
pose_tmp = pose[i]
print 'pose_tmp', pose_tmp
marker_name = 'realsense_camera' + str(i)
print 'marker_name', marker_name
int_marker = createInteractiveMarker(marker_name, *pose_tmp)
meshmarker = createMeshMarker(
'package://apc_config/models/cameras/realsense_camera_compressed.stl',
offset=tuple(pose_tmp[0:3]),
rgba=(0.1, 0.1, 0.1, 1),
scale=0.001,
orientation=tuple(pose_tmp[3:7]),
frame_id='/map')
int_marker.controls.append(meshmarker)
server.insert(int_marker, processFeedback)
pose = []
pose.append([1.3216 + .038, 0, 0.45089 + 0.015, 0, 0, 0, 1])
pose.append([1.3216 + .038, 0, 0.36449 + 0.015, 0, 0, 0, 1])
pose.append([0.5406 - .038, 0, 0.27902 + 0.015, 0, 0, 0, 1])
pose.append([0.48962 - .035821, 0, 0.042641 - 0.02, 0, 0, 0, 1])
for i in range(0, 4):
pose_temp = pose[i]
marker_name = 'camera_bar' + str(i)
int_marker = createInteractiveMarker(marker_name, *pose_temp)
meshmarker = createMeshMarker(
'package://apc_config/models/cameras/camera_bar.stl',
offset=tuple(pose_temp[0:3]),
rgba=(0.5, 0.5, 0.5, 1),
scale=0.001,
orientation=tuple(pose_temp[3:7]),
frame_id='/map')
int_marker.controls.append(meshmarker)
server.insert(int_marker, processFeedback)
#################
## Robot mount ##
#################
int_marker = createInteractiveMarker('Robot_mount', 0.02, 0, 0, 0.5, -0.5,
-0.5, 0.5)
meshmarker = createMeshMarker(
'package://apc_config/models/robot_mount/robot_mount_compressed.stl',
offset=(0.02, 0, 0),
rgba=(0.1, 0.1, 0.1, 1),
orientation=(0.5, -0.5, -0.5, 0.5))
int_marker.controls.append(meshmarker)
server.insert(int_marker, processFeedback)
if opt.useCamMarker or opt.useAllMarker:
int_marker = createInteractiveMarker('joint_realsense',
*pose_world,
frame_id='map')
viewMarker = createViewMarker(frame_id='realsense_depth_optical_frame')
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]
server.applyChanges()
# ros services
#startPoseService()
s1 = rospy.Service('manualfit_object_pose', GetPose, getPose)
rospy.spin()
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()
filename = os.environ['APCDATA_BASE']+'/capsen_vision/models/'+obj_id+'/model.pcd'
(points, colors) = load_pcd(filename)
pose = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw
if len(pose) <7:
pose = [0,0,0,0,0,0,1]
#pdb.set_trace()
# create an interactive marker server on the topic namespace simple_marker
server = InteractiveMarkerServer("affordance_marker")
int_marker = createInteractiveMarker('Object',*pose)
pointmarker = createPointMarker(colors=colors, points=points, offset=tuple(pose[0:3]), orientation=tuple(pose[3:7]))
int_marker.controls.append(pointmarker)
int_marker.controls.extend(createMoveControls(fixed=True))
#### create a static marker
dim = get_obj_dim(obj_id)
for i in range(5):
vizCubeMarker(size=dim, rgba=(0,1,0,0.3))
server.insert(int_marker, processFeedback)
rospy.Timer(rospy.Duration(0.01), frameCallback)
server.applyChanges()
# ros services
#startPoseService()
global q0
q0 = None
listener = tf.TransformListener()
rospy.sleep(0.1)
currentTargetPose = Pose()
br = TransformBroadcaster()
currentTargetPoseDirty = False
# create an interactive marker server on the topic namespace ik_interactive
server = InteractiveMarkerServer("ik_interactive")
# create an interactive marker for TargetPose
#pose = [1.1756, -0.0080496, 0.79966, 0.96262, -0.039399, -0.25934, 0.06743]
pose = [0, 0, 0, 0, 0, 0, 1]
currentTargetPose = rosposeTransform(poselist2pose(pose), 'link_6', 'map',
listener)
pose = pose2list(currentTargetPose)
int_marker = createInteractiveMarker('target_pose', *pose, 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)
int_marker.controls.extend(createMoveControls(fixed=False))
server.insert(int_marker, processFeedback)
rospy.Timer(rospy.Duration(0.1), frameCallback)
server.applyChanges()
print 'ready'
rospy.spin()
brLink6 = TransformBroadcaster()
brLink_tag = TransformBroadcaster()
# create an interactive marker server on the topic namespace simple_marker
server = InteractiveMarkerServer("urdf_marker")
# create an interactive marker for finger_left
trans = (0.0658272579312, -0.0897141247988, 0.256312400103)
qlink = tf.transformations.quaternion_from_euler(-math.pi/2, math.pi/2, 0) # put in mesh vs joint
int_marker = createInteractiveMarker('finger_left',
trans[0], trans[1], trans[2], qlink[0], qlink[1], qlink[2], qlink[3], frame_id='finger_left')
meshmarker = createMeshMarker('package://apc_config/models/wsg_50/meshes/SpatulaFingersTop.stl',
offset=trans, rgba=(0,1,0,1),
orientation=(qlink[0], qlink[1], qlink[2], qlink[3]), frame_id='/finger_left', scale=0.001)
int_marker.controls.append(meshmarker)
int_marker.controls.extend(createMoveControls())
#server.insert(int_marker, processFeedback)
# create an interactive marker for finger_right
trans = (-0.0974799394608, -0.131322726607, 0.111145801842)
qlink = tf.transformations.quaternion_from_euler(-math.pi/2, -math.pi/2, 0)
int_marker = createInteractiveMarker('finger_right',
trans[0], trans[1], trans[2], qlink[0], qlink[1], qlink[2], qlink[3], frame_id='finger_right')
meshmarker = createMeshMarker('package://apc_config/models/wsg_50/meshes/SpatulaFingersBottom.stl',
offset=trans, rgba=(0,1,0,1),
orientation=(qlink[0], qlink[1], qlink[2], qlink[3]), frame_id='/finger_right', scale=0.001)
int_marker.controls.append(meshmarker)
int_marker.controls.extend(createMoveControls())
server.insert(int_marker, processFeedback)
# create an interactive marker for wrist
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()
