class Kinect_controller:
#-------------------------------- init ----------------------------------------------------------------------------------
def __init__(self): # aqui podemos ponerle un parametro de entrada para que le enviemos el color que queramos que siga
rospy.init_node('Kinect_controller', anonymous=False)
rospy.loginfo("Initiating Kinect controller.")
rospy.loginfo("To stop Kinect controller CTRL + C")
rospy.on_shutdown(self.shutdown)
self.pub_blob = rospy.Publisher("blob_tracker", object_data, queue_size=10)
self.pub_depth = rospy.Publisher('depth_analysis', depth_data, queue_size=10)
self.pub_mea = rospy.Publisher('im2mea_transformation', mea_array, queue_size=10)
self.BD = Blob_detection()
self.DA = Depth_analysis()
self.I2M = Im2mea()
#-------------------- main function -------------------------------------------------------
#Actualiza el calculo del centroide del hueco y la detección de obstáculos mediante las clases Blob_detection y Depth_analysis.
def update(self):
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
# Get a fresh frame
depth,_ = get_depth()
frame,_ = get_video()
self.BD.detect(frame)
self.DA.analyse(depth)
self.I2M.find(self.BD.get_xpos())
centroid = self.DA.get_centroid()
obstacle = self.DA.get_obstacle()
object_located = self.BD.get_objectLocated()
mea = self.I2M.get_pos()
rospy.loginfo("centroid: %d, obstacle: %d", centroid, obstacle)
rospy.loginfo("object_located: %d", object_located)
rospy.loginfo("im2mea: \n %s", mea)
self.pub_depth.publish(centroid, obstacle)
self.pub_blob.publish(object_located)
self.pub_mea.publish(mea)
rate.sleep()
#-------------------------- aux functions ----------------------------------------------
def shutdown(self):
rospy.loginfo("Stop TurtleBot: Kinect controller node")
rospy.sleep(1)
def __init__(self): # aqui podemos ponerle un parametro de entrada para que le enviemos el color que queramos que siga
rospy.init_node('Kinect_controller', anonymous=False)
rospy.loginfo("Initiating Kinect controller.")
rospy.loginfo("To stop Kinect controller CTRL + C")
rospy.on_shutdown(self.shutdown)
self.pub_blob = rospy.Publisher("blob_tracker", object_data, queue_size=10)
self.pub_depth = rospy.Publisher('depth_analysis', depth_data, queue_size=10)
self.pub_mea = rospy.Publisher('im2mea_transformation', mea_array, queue_size=10)
self.BD = Blob_detection()
self.DA = Depth_analysis()
self.I2M = Im2mea()
