How to modify the code so that the robot picksup slowly

asked 2020-08-01 02:44:43 -0500

sree gravatar image

updated 2020-08-01 03:04:45 -0500

I want to make robot with soft pickup .i.e th robot sholud start slow and increase its speed slowly.HO wto modify the code. please help me out. here the speed is depends on twist.linear.x=0.2.if that is changed speed will changed .but I want to a soft start how to do that.

#!/usr/bin/env python
import rospy, cv2, numpy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError
import sys, select, termios, tty

bridge = CvBridge()
image = None


def image_callback(data):
    global image
    image = bridge.imgmsg_to_cv2(data, 'bgr8')

image_sub = rospy.Subscriber('/camera/rgb/image_raw', Image,image_callback)

def Image_converter():
    while True:
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        lower_yellow = numpy.array([ 10, 10, 10])
        upper_yellow = numpy.array([255, 255, 250])
        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
        h, w, d = image.shape
        search_top = 4*h/5
        search_bot = 4*h/5 + 20
        mask[0:search_top, 0:w] = 0
        mask[search_bot:h, 0:w] = 0
        M = cv2.moments(mask)
        if M['m00'] > 0:
            cx = int(M['m10']/M['m00'])
            cy = int(M['m01']/M['m00'])
            cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
            err = cx - w/2
            twist.linear.x = 0.2
            twist.angular.z = -float(err) / 1000
            cmd_vel_pub.publish(twist)
        cv2.imshow("window", image)
        cv2.waitKey(3)


if __name__ == '__main__':

    rospy.init_node('image_converter', anonymous=True)


    cmd_vel_pub = rospy.Publisher('~/cmd_vel_mux/input/teleop',Twist, queue_size=1)
    twist = Twist()

    Image_converter()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
        cv2.destroyAllWindows**
edit retag flag offensive close merge delete