In my algorithm, the robot seemingly moves towards the right position, as the joint velocities reduce, after that the velocities start increasing as the robot moves away. I cannot understand why this is happening and need some urgent help. My github project link is given below: https://github.com/RiddhimanRaut/Ur5_... The object recognition and everything else is working as expected. Also the robot does not stop when I stop the run_ur5.py code, I've checked rostopic echo and joint velocities are still being published. Maybe the two problems are related. Please help.

The codes are all mine.

EDIT 2:

This is a trial velocity publisher I've written just to test the lag issue

#!/usr/bin/env python
import rospy
from ur5_control_nodes.msg import floatList

import rospy
from std_msgs.msg import String

def talker():
    pub = rospy.Publisher('ur5_joint_velocities', floatList, queue_size=10)
    rospy.init_node('talker', anonymous=True)

    while not rospy.is_shutdown():
        vels=[0,0,0.05,0,0,0]

        pub.publish(vels)


if __name__ == '__main__':
    talker()

Code for color detection

#!/usr/bin/env python
import rospy
import numpy as np
import cv2
from sensor_msgs.msg import Image
from ur5_control_nodes.msg import floatList
from cv_bridge import CvBridge, CvBridgeError
import imutils

bridge=CvBridge()
rgb_img=np.zeros((480,640,3),np.uint8)
depth_img=np.zeros((480,640))

#RGB Image Callback
def rgb_callback(rgb_msg):
    global rgb_img   
    rgb_img=bridge.imgmsg_to_cv2(rgb_msg, "bgr8")     


#Depth Image Callback 
def d_callback(msg):

    global depth_img
    depth_img=bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")




#Getting XY Points
def xy_points(frame):
    fl=531.15

    xypoints=np.array([0,0,0,0,0,0], dtype=np.int64)


    blurred = cv2.GaussianBlur(frame, (11, 11), 0)

    hsv=cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)

    #defining the Range of Blue color
    blue_lower=np.array([87,100,150],np.uint8)
    blue_upper=np.array([110,255,255],np.uint8)



    blue=cv2.inRange(hsv,blue_lower,blue_upper)
    blue = cv2.erode(blue, None, iterations=2)
    blue = cv2.dilate(blue, None, iterations=2)

    cnts_b = cv2.findContours(blue.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)
    cnts_b = imutils.grab_contours(cnts_b)
    center_b = None
    if len(cnts_b) > 0:
        c_b = max(cnts_b, key=cv2.contourArea)
        # epsilon = 0.1*cv2.arcLength(c,True)
        # approx = cv2.approxPolyDP(c,epsilon,True)
        M_b= cv2.moments(c_b)
        center_b= (int(M_b["m10"] / M_b["m00"]), int(M_b["m01"] / M_b["m00"]))
        cv2.circle(frame, center_b, 5, (255, 255,255), -1)
        xypoints[2]=center_b[0]
        xypoints[3]=center_b[1]
    #Red
    low_r = np.array([140,150,0],np.uint8)
    high_r = np.array([180,255,255],np.uint8)

    red=cv2.inRange(hsv,low_r,high_r)
    red = cv2.erode(red, None, iterations=2)
    red = cv2.dilate(red, None, iterations=2)

    cnts_r = cv2.findContours(red.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)
    cnts_r = imutils.grab_contours(cnts_r)
    center_r = None
    if len(cnts_r) > 0:
        c_r = max(cnts_r, key=cv2.contourArea)
        # epsilon = 0.1*cv2.arcLength(c,True)
        # approx = cv2.approxPolyDP(c,epsilon,True)
        M_r= cv2.moments(c_r)
        center_r= (int(M_r["m10"] / M_r["m00"]), int(M_r["m01"] / M_r["m00"]))
        cv2.circle(frame, center_r, 5, (255, 255,255), -1)
        xypoints[4]=center_r[0]
        xypoints[5]=center_r[1]

    #Green
    low_g = np ...