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 run(self): rate = rospy.Rate(self.frequency) #czestotliwosc pracy wezla self.irpos.set_tool_physical_params(10.8, Vector3(0.004, 0.0, 0.156)) #ustawienia do sterowania predkoscia pub = rospy.Publisher('uchyb', Float32, queue_size=10) #publikacja uchybu do celu rysowania wykresy rospy.Subscriber("object_seen_by_camera", SerwoInfo, self.callback) #subskrybcja T_gC_vector while not rospy.is_shutdown(): if not not self.T_gC_vector: self.T_gC = matrixOperations.translation_from_vector(self.T_gC_vector) #przeksztalcenie wektora na macierz self.regulatroDecomposition() self.calculateAndSetNewValues() self.irpos.start_force_controller(Inertia(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), ReciprocalDamping(Vector3(0.0025, 0.0025, 0.0025), Vector3(0.0, 0.0, 0.0)), Wrench(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Twist(Vector3(self.newVel[0], self.newVel[1], self.newVel[2]), Vector3(self.newVel[3], self.newVel[4], self.newVel[5]))) #sterowanie predkoscia rate.sleep() pub.publish(self.error[1]) #publikacja uchybu self.irpos.stop_force_controller()
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()