コード例 #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()
コード例 #2
0
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()