my new robot (rabbot) is moving very very slow, an the old robot (turtlebot) was moving fast by the same code

edit

Hi everyone

I am working with the human following (object detection like Yolo and SSD ) using a new robot (rabbot) but it moves very very slow with any code when there is this line

rospy.spin()

this is my full code:

import rospy
import cv2
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist
from imutils.video import FPS
import time

global net
net = cv2.dnn.readNetFromCaffe('prototxt.txt', 'caffemodel')

global counter
global count_max
counter = 0
count_max = 400000

# video writer setup
global fourcc
global out
fourcc = cv2.VideoWriter_fourcc('M','J','P','G') #Define the codec and create VideoWriter object
out = cv2.VideoWriter('following_human.avi',fourcc, 20.0, (640,480))

class CVControl:
    def __init__(self):


        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.cmd = Twist()

        # Image subscriber
        self.bridge = CvBridge()

        self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.img_callback)

    def img_callback(self, data):
        global counter ,v,w
        global net
        global out
        global count_max
        global startX, startY, endX, endY 
        counter += 1


        CLASSES = ["background", "aeroplane", "bicycle", "bird", "boat",
                    "bottle", "bus", "car", "cat", "chair", "cow", "diningtable",
                    "dog", "horse", "motorbike", "person", "pottedplant", "sheep",
                    "sofa", "train", "tvmonitor"]
        COLORS = np.random.uniform(0, 255, size=(len(CLASSES), 3))

        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print e
        cv_gray = cv2.cvtColor(cv_image,cv2.COLOR_BGR2GRAY)
        frame = cv_image

        (h, w1) = frame.shape[:2]
        blob = cv2.dnn.blobFromImage(cv2.resize(frame, (300, 300)),0.007843, (300, 300), 127.5)
        fps = FPS().start()
        (h, w1) = frame.shape[:2]
        # pass the blob through the network and obtain the detections and
                # predictions
        net.setInput(blob)
        detections = net.forward()

        big_area = 0
        big_center = 320
        detected = 0

        for i in np.arange(0, detections.shape[2]):
            object_type = detections[0,0,i,1]
            confidence = detections[0, 0, i, 2]
            if object_type == 15 and confidence > 0.2: 


                box = detections[0, 0, i, 3:7] * np.array([w1, h, w1, h])
                (startX, startY, endX, endY) = box.astype("int")


                label = "{}: {:.2f}%".format('person',confidence * 100)
#                cv2.rectangle(frame, (startX, startY), (endX, endY),[0,0,255], 2)
                y = startY - 15 if startY - 15 > 15 else startY + 15
                cv2.putText(frame, label, (startX, y),cv2.FONT_HERSHEY_SIMPLEX, 0.5, [0,0,255], 2)


                rect_center = int((startX+endX)/2)
                rect_area = (endX-startX)*(endY-startY)
                detected = 1
                if rect_area > big_area: 
                    big_area = rect_area
                    big_center = rect_center


        if detected:
            cv2.rectangle(frame, (startX, startY), (endX, endY),[0,0,255], 2)

            if big_area > 10000: # Execute if the person is within a reasonable range
                target_center = 320
                target_area = 150000

                kr = .002
                w = -kr*(big_center - target_center)
                kt = 0.0000045
#               
                v = -kt*(big_area - target_area)
                maxv = 1 
                v = np.max([-maxv, v])
                v = np.min([maxv, v])


                self.send_command(v, w) 


        if counter < count_max:
            out.write(frame)
            print(counter)
        if counter == count_max:
            out.release()
            print('made video')
        cv2.imshow("Image window", frame)
        cv2.waitKey(3)
        fps.update()
        fps.stop()
        print("[INFO] elapsed time: {:.2f}".format(fps.elapsed()))
        print("[INFO] approx. FPS: {:.2f}".format(fps.fps()))
    def send_command(self, v, w):

        self ...