rospy.init_node("find_model_transform")
br = TransformBroadcaster()
obj_id = sys.argv[1] # like oreo_mega_stuf
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)
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()
currentJoint4 = Pose()
currentJoint5 = Pose()
currentJoint6 = Pose()
currentJoint_tag = Pose()
brLink4 = TransformBroadcaster()
brLink5 = TransformBroadcaster()
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)
rospy.init_node("find_model_transform")
br = TransformBroadcaster()
obj_id = sys.argv[1] # like oreo_mega_stuf
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)
if __name__=="__main__":
rospy.init_node("ik_interactive_server")
global currentTargetPose, currentTargetPoseDirty
global br
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]
currentTargetPose = poselist2pose( pose )
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()
