Delay video when calling to move

edit

Hi. I have the following code that displays the compressed video from a robot and moves the robot (move_robot.py). There is a noticeable delay (3 seconds) when looking at the displayed video and what is happening in real-time. I started commenting line by line to see the cause of this delay.

The following line is the reason of the delay:

> self.movekobuki_object.move_robot(twist_object)

When I use this other python code ([movekobuki] from move_robot.py) which moves the robot, it delays the video time by the 3 seconds. Is there something I do wrong here? is it the rate? I can't find a solution to this...

     #!/usr/bin/env python

     import rospy import cv2 import numpy
     as np from cv_bridge import CvBridge,
     CvBridgeError from geometry_msgs.msg
     import Twist from sensor_msgs.msg
     import Image, CompressedImage from
     move_robot import MoveKobuki from
     pid_control import PID import time



     class LineFollower(object):

         def __init__(self):

             rospy.logwarn("Init line Follower")
             self.bridge = CvBridge() 
             self.image_sub = rospy.Subscriber('/camera/image_raw/compressed',CompressedImage, self.camera_callback,
                      queue_size = 1)
             self.movekobuki_object = MoveKobuki()
             self.pid_object = PID()

         def camera_callback(self,data):

             try:
                 image_np = self.bridge.compressed_imgmsg_to_cv2(data)
             except CvBridgeError as e:
                 print(e)


             height, width, channels = image_np.shape
             descentre = 100 # 50 works
             rows_to_watch = 100 # 15 works
             crop_img = image_np[(height)/2+descentre:(height)/2+(descentre+rows_to_watch)][1:width]

             # Blur Image (to help with color noise)
             blur = cv2.medianBlur(crop_img,15)

             # Convert from RGB to HSV
             hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)


             # Threshold the HSV image to get only yellow colors
             mask = cv2.inRange(hsv, lower_yellow, upper_yellow)





             # Calculate centroid of the blob of binary image using ImageMoments

             m = cv2.moments(mask, False)
             try:
                cx, cy = m['m10']/m['m00'], m['m01']/m['m00']
             except ZeroDivisionError:
                 cx = 240
                 cy = 320

             # Bitwise-AND mask and original image
             res = cv2.bitwise_and(crop_img,crop_img,mask= mask)

             cv2.circle(res, (int(cx), int(cy)), 10,(0,0,255),-1)

             cv2.imshow("Full Img", image_np)


             cv2.waitKey(1)

             # Move the Robot , center it in the middle of the width 640 => 320:
             setPoint_value = width/2
             self.pid_object.setpoint_update(value=setPoint_value)

             twist_object = Twist();
             twist_object.linear.x = 0.0; #0.2

             # make it start turning

             self.pid_object.state_update(value=cx)
             effort_value = self.pid_object.get_control_effort()

             angular_effort_value = 0 #effort_value / 350.0 # was 200 and 300 was latest

             twist_object.angular.z = angular_effort_value;
             self.movekobuki_object.move_robot(twist_object)




         def clean_up(self):
             self.movekobuki_object.clean_class()
             cv2.destroyAllWindows()

 def main():
         rospy.init_node('line_following_node', anonymous=True)
         line_follower_object = LineFollower()

         rate = rospy.Rate(30) #originally 5
         ctrl_c = False
         def shutdownhook():
             line_follower_object.clean_up()
             rospy.loginfo("shutdown time!")
             ctrl_c = True

         rospy.on_shutdown(shutdownhook)

         while not ctrl_c:
             rate.sleep()

     if __name__ == '__main__':
         main()

The move_robot.py is as follows, something here is

    #!/usr/bin/env python
    import rospy
    from geometry_msgs.msg import Twist
    class MoveKobuki(object):

        def __init__(self):

            self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
            self.cmd_vel_subs = rospy.Subscriber('/cmd_vel', Twist, self.cmdvel_callback)
            self.last_cmdvel_command = Twist()
            self._cmdvel_pub_rate = rospy.Rate(10) #10 is original
            self.shutdown_detected = False

        def cmdvel_callback(self, msg):
            self.last_cmdvel_command = msg

        def compare_twist_commands(self, twist1, twist2):
            LX ...