def talker():
pub = rospy.Publisher('object_seen_by_camera', SerwoInfo, queue_size=10)
rospy.init_node('object_seen_by_camera', anonymous=True)
msg = SerwoInfo()
vector_gB = []
vector_gC = []
listener = tf.TransformListener()
a = np.empty([4, 4])
b = np.empty([4, 4])
rate = rospy.Rate(500)
listener.waitForTransform('/p_c_optical_frame', '/world', rospy.Time(0), rospy.Duration(10))
while not rospy.is_shutdown():
#T_bC pozycja /wordl (B) w ukladzie /p_c_optical_frame (C)
(trans,rot) = listener.lookupTransform('/p_c_optical_frame', '/world', rospy.Time(0))
T_bC = matrixOperations.euler_matrix_from_quaternion(rot, trans)
y = math.sin(rospy.get_time()/5)
y = 2.3 + y/5
vector_gB = [1,0,0,0.88, 0,1,0,y, 0,0,1,0.6, 0,0,0,1]
T_gB = matrixOperations.translation_from_vector(vector_gB)
a = T_gB
T_gC = np.dot(T_bC , T_gB)
vector_gC = matrixOperations.vector_from_translation(T_gC)
msg.matrix = vector_gC;
msg.found = 1;
msg.out_time_nsec_pocz = 0;
msg.out_time_sec_pocz = 0;
msg.out_time_nsec_kon = 0;
msg.out_time_sec_kon = 0;
pub.publish(msg)
rate.sleep()
def realGrid():
rate = rospy.Rate(500)
pawn = Marker()
pawn.header.frame_id = "/world"
pawn.type = pawn.CUBE
pawn.action = pawn.ADD
listener = tf.TransformListener()
listener.waitForTransform('/p_c_optical_frame', '/world', rospy.Time(0), rospy.Duration(10))
#T_bC pozycja /wordl (B) w ukladzie /p_c_optical_frame (C)
(trans,rot) = listener.lookupTransform('/p_c_optical_frame', '/world', rospy.Time(0))
T_bC = matrixOperations.euler_matrix_from_quaternion(rot, trans)
T_gC = matrixOperations.translation_from_vector(T_gC_vector)
T_gB = np.dot(np.linalg.inv(T_bC) , T_gC)
print T_gB
global realHomogMatrix
realHomogMatrix = T_gB
T = PyKDL.Frame(PyKDL.Rotation(realHomogMatrix[0][0], realHomogMatrix[0][1], realHomogMatrix[0][2],realHomogMatrix[1][0], realHomogMatrix[1][1], realHomogMatrix[1][2], realHomogMatrix[2][0], realHomogMatrix[2][1], realHomogMatrix[2][2]))
q = T.M.GetQuaternion()
pawn.pose = Pose(Point(newPoint[0],newPoint[1],newPoint[2]), Quaternion(q[0],q[1],q[2],q[3]))
pawn.color.a = 1.0
pawn.color.r = 1.0
pawn.color.g = 1.0
pawn.color.b = 0.0
pawn.scale.x = 0.2
pawn.scale.y = 0.4
pawn.scale.z = 0.01
pawn.scale.x = 0.2
pawn.scale.y = 0.4
pawn.scale.z = 0.01
publisher.publish(pawn)
rate.sleep()
