Slow data rate from Camera vision

edit

I am using a Raspberry Pi 3B with Pi Camera and I set up the python code below to help me compute the position of a robot and publish it in order to use the data to control the robot. But I have an issue with the C++ code acting on those data. The code reacts way faster than the data are published making the robot to always stop ahead of the target position.

I am using the published data in a while loop in the C++ node controlling the robot. I tried to slow down the speed of the loop, but that does not seem to help.

Could you help me see through this?

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32

# import the necessary packages
from picamera.array import PiRGBArray
from picamera import PiCamera
from collections import deque
import datetime
import time
import cv2
import argparse
# import imutils  (removed)

# Math functions
import numpy as np

pub_x = rospy.Publisher('ball_pose_x', Float32, queue_size=10)
pub_y = rospy.Publisher('ball_pose_y', Float32, queue_size=10)
rospy.init_node('ball_tracking')

# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(640, 480))

# allow the camera to warmup
time.sleep(0.1)

# ----- Basic definition -----------

# Args definition
# construct the argument parse and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video",
    help="path to the (optional) video file")
ap.add_argument("-b", "--buffer", type=int, default=64,
    help="max buffer size")
args = vars(ap.parse_args())


# define the lower and upper boundaries of the
# ball in the HSV color space, then initialize the
# list of tracked points
whiteLower = (0, 0, 230)
whiteUpper = (180, 25, 255)
pts = deque(maxlen=args["buffer"])

# ------ end definition ----------

# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
    # grab the raw NumPy array representing the image, then initialize the timestamp
    # and occupied/unoccupied text
    image = frame.array
    # print " "
    # print datetime.datetime.now()
    # -------------------- Modif start------------------

    # blurred = cv2.GaussianBlur(frame, (11, 11), 0)
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

    # construct a mask for the color "white", then perform
    # a series of dilations and erosions to remove any small
    # blobs left in the mask
    mask = cv2.inRange(hsv, whiteLower, whiteUpper)
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)

    # find contours in the mask and initialize the current
    # (x, y) center of the ball
    cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)[-2]
    center = None

    # only proceed if at least one contour was found
    if len(cnts) > 0:
        # find the largest contour in the mask, then use
        # it to compute the minimum enclosing circle and
        # centroid
        c = max(cnts, key=cv2.contourArea)
        ((x, y), radius) = cv2.minEnclosingCircle(c)
        M = cv2.moments(c)
        center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

        # Publishing data to ROS
        # print datetime.datetime.now()
        pub_x.publish(M["m10"] / M["m00"])
        pub_y.publish(M["m01"] / M["m00"])

        # only proceed if the radius meets ...