ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Two subscribers, one with while loop, another cannot receive new ros message

asked 2019-11-12 18:38:37 -0500

henrykim gravatar image

updated 2019-11-13 09:33:04 -0500

Hello, I am trying to make a program that starts reading camera frames with opencv and ros. Currently, I begin reading camera frames from raspberry pi camera when "start" signal is subscribed. "start" signal and opencv frame are published to another topic.

At some time, I send "end" signal to the subscriber. However, the subscriber cannot read new signal. When "start" signal came at the beginning, the callback function runs get_frame function and this function goes through while loop. So, new signals are not received at all.

class GetFrame:
def __init__(self, node_name):
    self.operate = rospy.Subscriber('operate', operate, self.callback)
    self.pub_frame = rospy.Publisher('img_camera', sendframe, queue_size=3)
    self.node_name = node_name
    self.net = None
    self.detect = None
    self.cap = None
    self.args = arg_parse()
    self.frame_data = sendframe()
    self.bridge = CvBridge()

def callback(self, data):
    if data.command == "init":
        self.initialize()
    elif data.command == "start":
        self.get_frame(data)
    elif data.command == "end":
        self.get_frame(data)

def initialize(self):
    self.net = cv2.dnn.readNetFromDarknet(self.args.config, self.args.weight)
    self.net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV)
    self.net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU)

    # load DetectBoxes class
    self.detect = DetectBoxes(self.args.labels,
                              confidence_threshold=self.args.confidence,
                              nms_threshold=self.args.nmsThreshold)

    try:
        self.cap = cv2.VideoCapture(0)
    except IOError:
        sys.exit(1)

@staticmethod
def get_outputs_names(net):
    # names of network layers e.g. conv_0, bn_0, relu_0....
    layer_names = net.getLayerNames()
    return [layer_names[i[0] - 1] for i in net.getUnconnectedOutLayers()]

def get_frame(self, operate):
    # if start signal came before init signal
    # initialize net & camera
    if self.cap is None or not self.cap.isOpened():
        self.initialize()

    while self.cap.isOpened():
        has_frame, frame = self.cap.read()

        # if end of frame, program is terminated
        if not has_frame:
            break

        # Create a 4D blob from a frame.
        blob = cv2.dnn.blobFromImage(frame, 1 / 255, (self.args.resol, self.args.resol),
                                     (0, 0, 0), True, crop=False)

        # Set the input to the network
        self.net.setInput(blob)

        # Runs the forward pass
        network_output = self.net.forward(self.get_outputs_names(self.net))

        # Extract the bounding box and draw rectangles
        self.frame_data.percent, self.frame_data.coords = self.detect.detect_bounding_boxes(frame, network_output)

        print("FPS {:5.2f}".format(1000/elapsed))

        # publish frames + detected objects
        self.frame_data.operate = operate.command
        try:
            self.frame_data.frame = self.bridge.cv2_to_imgmsg(frame, encoding="passthrough")
            self.pub_frame.publish(self.frame_data)
        except CvBridgeError as e:
            print(e)

    self.log.publish(log_generator(self.node_name, "Camera closed"))
    # releases video and removes all windows generated by the program
    # cap.release()


 if __name__ == '__main__':
     rospy.init_node('get_frame', anonymous=True)
     g_frame = GetFrame('get_frame')
     try:
         rospy.spin()
     except KeyboardInterrupt:
         print("Shut down - keyboard interruption")

Should I stop while loop? How can I subscribe new message while not ending while loop?

Thank you

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-11-13 00:39:42 -0500

duck-development gravatar image

updated 2019-11-13 12:18:41 -0500

Because the start is done in a other thread then the stop.

If you like to stop the start thread you have to use the "data" as an global var

So in the constructur you write

try this:

class GetFrame:
def __init__(self, node_name):
    self.operate = rospy.Subscriber('operate', operate, self.callback)
    self.pub_frame = rospy.Publisher('img_camera', sendframe, queue_size=3)
    self.node_name = node_name
    self.net = None
    self.detect = None
    self.cap = None
    self.args = arg_parse()
    self.frame_data = sendframe()
    self.bridge = CvBridge()
    self.cmd = "end"

def callback(self, data):
    self.cmd = data.command 
    if data.command == "init":
        self.initialize()
    elif data.command == "start":
        self.get_frame()


def initialize(self):
    self.net = cv2.dnn.readNetFromDarknet(self.args.config, self.args.weight)
    self.net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV)
    self.net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU)

    # load DetectBoxes class
    self.detect = DetectBoxes(self.args.labels,
                              confidence_threshold=self.args.confidence,
                              nms_threshold=self.args.nmsThreshold)

    try:
        self.cap = cv2.VideoCapture(0)
    except IOError:
        sys.exit(1)

@staticmethod
def get_outputs_names(net):
    # names of network layers e.g. conv_0, bn_0, relu_0....
    layer_names = net.getLayerNames()
    return [layer_names[i[0] - 1] for i in net.getUnconnectedOutLayers()]

def get_frame(self):
    # if start signal came before init signal
    # initialize net & camera
    if self.cap is None or not self.cap.isOpened():
        self.initialize()

    while self.cap.isOpened():
        if self.cmd != "start"
            return

        has_frame, frame = self.cap.read()

        # if end of frame, program is terminated
        if not has_frame:
            break

        # Create a 4D blob from a frame.
        blob = cv2.dnn.blobFromImage(frame, 1 / 255, (self.args.resol, self.args.resol),
                                     (0, 0, 0), True, crop=False)

        # Set the input to the network
        self.net.setInput(blob)

        # Runs the forward pass
        network_output = self.net.forward(self.get_outputs_names(self.net))

        # Extract the bounding box and draw rectangles
        self.frame_data.percent, self.frame_data.coords = self.detect.detect_bounding_boxes(frame, network_output)

        print("FPS {:5.2f}".format(1000/elapsed))

        # publish frames + detected objects
        self.frame_data.operate = cmd
        try:
            self.frame_data.frame = self.bridge.cv2_to_imgmsg(frame, encoding="passthrough")
            self.pub_frame.publish(self.frame_data)
        except CvBridgeError as e:
            print(e)

    self.log.publish(log_generator(self.node_name, "Camera closed"))
    # releases video and removes all windows generated by the program
    # cap.release()


 if __name__ == '__main__':
     rospy.init_node('get_frame', anonymous=True)
     g_frame = GetFrame('get_frame')
     try:
         rospy.spin()
     except KeyboardInterrupt:
         print("Shut down - keyboard interruption")
edit flag offensive delete link more

Comments

Thank you for the reply, but this didn't solve my problem. My problem is that at first Subscriber receives "start" signal so through callback function is starts get_frame function. SInce there is a while loop inside get_frame it does not end. Therefore, even though "end" signal is published the subscriber is not reading the "end" signal, it simply continues processing while loop.

henrykim gravatar image henrykim  ( 2019-11-13 09:29:24 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2019-11-12 18:38:37 -0500

Seen: 237 times

Last updated: Nov 13 '19