Example #1
0
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 talker():
    pub = rospy.Publisher('realHomogMatrix', SerwoInfo, queue_size=10)
    rospy.init_node('realHomogMatrix', anonymous=True)
    msg = SerwoInfo()
    matrix = []

    #w rzeczywistosci obiekt poruszasza sie z taka czestotliwoscia
    rate = rospy.Rate(500) 
    while not rospy.is_shutdown():
        y = math.sin(rospy.get_time()/5)
        y = 2.2 + y/4
        matrix = [1,0,0,0,0,1,0,y,0,0,1,0,0,0,0,1]
        msg.matrix = matrix;
        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()