Revision history [back]

It works fine now

#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import division
from __future__ import absolute_import
import rospy
import cv2
from std_msgs.msg import String, Float32MultiArray, Bool, Float32
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image, PointCloud2

class Detector:
    def __init__(self):
        self.pubtracking = rospy.Publisher('control', Float32MultiArray, queue_size=1)
        rospy.Timer(rospy.Duration(1 / 50), self.control)

        self._bridge = CvBridge()
        # rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback)
        # rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
        rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback)
        # rospy.Subscriber('/camera/depth/points', PointCloud2, self.pc_callback)
        # rospy.Subscriber('stopback', Bool, self.shutdownlistener)
        self._current_image = None
        self._current_pc = None
        # self.array = 0
        self.error_v = 0
        self.error_w = 0


    def image_callback(self, image):
        self._current_image = image
    def run(self):
        global error_v, error_w, array
        rate = rospy.Rate(30)
        while not rospy.is_shutdown():
            if self._current_image is not None:
                try:
                    scene = self._bridge.imgmsg_to_cv2(self._current_image, "bgr8")
                    self.error_v = 2
                    self.error_w = 1
                    # hello_str = [error_v, error_w]
                    # array = Float32MultiArray(data=hello_str)
                    # self.control()

                    cv2.imshow("image", scene)
                    key = cv2.waitKey(1)
                    rate.sleep()
                except CvBridgeError as e:
                    print(e)

    def control(self, event=None):
        # error_v = 2
        # error_w = 1
        hello_str = [self.error_v, self.error_w]
        print hello_str
        array = Float32MultiArray(data=hello_str)
        self.pubtracking.publish(array)
        # rospy.Timer(rospy.Duration(1 / 30), self.control)
        # rospy.spin()





if __name__ == '__main__':
    rospy.init_node('dodo_detector_ros', log_level=rospy.INFO)
    try:
        tr = Detector()
        tr.run()
        # rospy.spin()
        # tr.control()
    except KeyboardInterrupt:
        rospy.loginfo('Shutting down')