turtlebot continues driving after Ctrl + C

edit

Hey Everybody,

I have a Turtlebot3 Waffle Pi, everything is setup as told in the official Tutorial. My Remote PC is a Ubuntu 16.04. VM, ROS Kinetic installed on the turtlebot. Currently I am working on a Lane Following Script in Python which is working quite well so far. The only thing I do have trouble with, is that the turtlebot continues driving, even after I shut down the Script with Ctrl + c. I checked via rostopic echo /cmd_vel if their are somehow still messages sent, but traffic immediately stops after the script is shutdown. My suggestion is that somehow the turtlebot buffers the last received message. I am using callback functions and rospy.spin() as mentioned in the rospy shutdown tutorial but somehow my turtlebot still keeps on moving after exiting the program.

Code is as follows:

import rospy, cv2, numpy as np, cv_bridge, time, sys
from sensor_msgs.msg import Image, CompressedImage 
from geometry_msgs.msg import Twist

twist_publisher = rospy.Publisher('cmd_vel', Twist, queue_size = 5)
image_sub = rospy.Subscriber("/raspicam_node/image/compressed", CompressedImage, onNewImage)
err = 0.0
pastError = 0
totalError = 0

def speedControl(errlow):
    global pastError
    global totalError

    kp = 0.8
    kd = 0.0

    if errlow > -50 and errlow <  50:
        twist_value = 0.25
    elif errlow > -100 and errlow < 100:
        twist_value = 0.2
    elif errlow >  -150 and errlow < 150:
        twist_value = 0.15

    return (twist_value)

def steeringControl(errlow):
    global pastError
    global totalError

    kp = 0.8
    kd = 0.0 

    value = kp * errlow + kd * pastError
    pastError = errlow
    totalError = (errlow - pastError)

    return (-float(value) / 500)

def onNewImage(imagedata):
    global twist_publisher
    global err


    bridge = cv_bridge.CvBridge()
    twist = Twist()

    image = bridge.compressed_imgmsg_to_cv2(imagedata ,desired_encoding='bgr8')

    frame = cv2.GaussianBlur(image, (5, 5), 0)
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    l_mask = cv2.inRange(gray, 190, 255)
    view = cv2.inRange(gray, 190, 255)

    h, w, d = image.shape

    search_bot_high = 2*h/4
    search_bot_low = 2*h/4 + 20

    l_mask[0:search_bot_high, 0:w] = 0
    l_mask[search_bot_low:h, 0:w] = 0

    L = cv2.moments(l_mask)

    if L['m00'] > 0:
            cx = int(L['m10']/L['m00'])
            cy = int(L['m01']/L['m00'])

            last_dir = cv2.GaussianBlur(image, (5, 5), 0)
            err = cx - w/2                

            twist.angular.z = steeringControl(err)
            #twist.linear.x = speedControl(err)
            twist.linear.x = 0.1

    twist_publisher.publish(twist)

    print "Twist x: " + str(twist.linear.x)
    print "Twist z: " + str(twist.angular.z)
    print " "

def initRos():
    global twist_publisher

    image_sub = rospy.Subscriber("/raspicam_node/image/compressed", CompressedImage, onNewImage)
    twist_publisher = rospy.Publisher("/cmd_vel", Twist, queue_size = 5)

if __name__ == '__main__':
    rospy.init_node('my_line_follower')
    initRos()
    rospy.spin()

Output of rostopic list

/battery_state
/cmd_vel 
/cmd_vel_rc100
/diagnostics
/firmware_version
/imu
/joint_states 
/magnetic_field
/motor_power
/odom
/raspicam_node/camera_info
/raspicam_node/image/compressed
/raspicam_node/parameter_descriptions
/raspicam_node/parameter_updates
/reset
/rosout
/rosout_agg
/rpms
/scan
/sensor_state
/sound
/steering_image
/tf
/version_info
/view_image

Any help is much appreciated.