Merge pull request #14 from KadenKing/Kaden_Vision

Kaden vision: update

bog/nodes/seeker.py

@@ -6,8 +6,8 @@
 from std_msgs.msg import String
 from std_msgs.msg import Float32
 
-CASCADE_PATH  = "/home/kaden/Documents/button/src/buttonseek/src/cascade_19.xml" #absolute path to the cascade
-DEBUG_DISPLAY = 0 # if true, shows the camera and its interpretations on the picture
+CASCADE_PATH  = "/home/uf-ieee/Documents/ws/src/IEEE2018/bog/nodes/cascade_19.xml" #absolute path to the cascade
+DEBUG_DISPLAY = 1 # if true, shows the camera and its interpretations on the picture
 
 def vision():
     direct = rospy.Publisher('direction', String, queue_size=10) #handles the publishing of the direction of the button
@@ -17,7 +17,7 @@ def vision():
 
     button_cascade = cv2.CascadeClassifier(CASCADE_PATH) # uses the haar cascade in the path to find the button
 
-    cap = cv2.VideoCapture(1) # cap is the image that the camera captured
+    cap = cv2.VideoCapture(0) # cap is the image that the camera captured
 
 
     while not rospy.is_shutdown():
@@ -33,7 +33,7 @@ def vision():
         # add this
         # image, reject levels level weights.
         #buttons = button_cascade.detectMultiScale(gray, scaleFactor=2 ,minNeighbors=30)
-        buttons = button_cascade.detectMultiScale(gray, scaleFactor=1.2 ,minNeighbors=30) # adjusts how sensitive the cascade is to saying that there is a button in the frame
+        buttons = button_cascade.detectMultiScale(gray, scaleFactor=1.1 ,minNeighbors=30) # adjusts how sensitive the cascade is to saying that there is a button in the frame
 
         # add this
         for (x,y,w,h) in buttons:

bog/nodes/vision_control.py

@@ -1,48 +1,147 @@
 #!/usr/bin/env python
 import rospy
+import time
 from std_msgs.msg import String
 from std_msgs.msg import Float32
 from bog.msg import SetWheelSpeeds
 
 #constants
 DISTANCE_RATIO = 0.25 #this constant will be used to determine how much to move the robot left or right. The farther away we are, the less we want to move left or right. 
-GOAL_DISTANCE_LOW = 9 # the lower bound of the goal distance from the button
+GOAL_DISTANCE_LOW = 5 # the lower bound of the goal distance from the button
 GOAL_DISTANCE_HIGH = 11 # the upper bound of the goal distance from the button
-
+WHEEL_SPEED = 50
 
 #state variables
 current_direction = ""
 current_distance = -1 
-
+updated2 = False
 
 #move method will move the robot left, right, or forward depending on the distance and direction passed to it.
 #we will move toward the button until certain parameters are met
+
+wheels = SetWheelSpeeds()
+
+class status:
+    updated = False
+
+
+def forward():
+    pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10)
+    wheels.wheel1 = WHEEL_SPEED
+    wheels.wheel2 = WHEEL_SPEED
+    wheels.wheel3 = WHEEL_SPEED
+    wheels.wheel4 = WHEEL_SPEED
+    pub.publish(wheels)
+    time.sleep(0.5)
+    wheels.wheel1 = 0
+    wheels.wheel2 = 0
+    wheels.wheel3 = 0
+    wheels.wheel4 = 0
+    pub.publish(wheels)
+    time.sleep(0.5)
+
+def backward():
+    pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10)
+    wheels.wheel1 = -1*WHEEL_SPEED
+    wheels.wheel2 = -1*WHEEL_SPEED
+    wheels.wheel3 = -1*WHEEL_SPEED
+    wheels.wheel4 = -1*WHEEL_SPEED
+    pub.publish(wheels)
+    time.sleep(0.5)
+    wheels.wheel1 = 0
+    wheels.wheel2 = 0
+    wheels.wheel3 = 0
+    wheels.wheel4 = 0
+    pub.publish(wheels)
+    time.sleep(0.5)
+
+
+def left():
+    pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10)
+    wheels.wheel1 = 1*WHEEL_SPEED
+    wheels.wheel2 = -1*WHEEL_SPEED
+    wheels.wheel3 = 1*WHEEL_SPEED
+    wheels.wheel4 = -1*WHEEL_SPEED
+    pub.publish(wheels)
+    time.sleep(0.5)
+    wheels.wheel1 = 0
+    wheels.wheel2 = 0
+    wheels.wheel3 = 0
+    wheels.wheel4 = 0
+    pub.publish(wheels)
+    time.sleep(0.5)
+
+def right():
+    pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10)
+    wheels.wheel1 = -1*WHEEL_SPEED
+    wheels.wheel2 = 1*WHEEL_SPEED
+    wheels.wheel3 = -1*WHEEL_SPEED
+    wheels.wheel4 = 1*WHEEL_SPEED
+    pub.publish(wheels)
+    time.sleep(0.5)
+    wheels.wheel1 = 0
+    wheels.wheel2 = 0
+    wheels.wheel3 = 0
+    wheels.wheel4 = 0
+    pub.publish(wheels)
+    time.sleep(0.5)
+
+
+
+
 def move(direction, distance): 
-    if current_distance < GOAL_DISTANCE_LOW:
-        rospy.loginfo("test")
+
+    rospy.loginfo("Direction: %s", current_direction) # write the information to the console
+    rospy.loginfo("Distance: %s", current_distance) # write the information to the console
+
+    if direction == "left":
+        left()
+    elif direction == "right":
+        right()
+    elif current_distance > GOAL_DISTANCE_LOW: #move forward
+        forward()
+    elif current_distance > GOAL_DISTANCE_HIGH:
+        backward()
+
+
+
+
+
 
 
 def distance_callback(data):
+    global current_distance
     current_distance = data.data # sets current distance to the updated value of the distance topic
-    rospy.loginfo("Distance: %s", current_distance) # write the information to the console
-    updated = True
+    #rospy.loginfo("Distance: %s", current_distance) # write the information to the console
+    status.updated = True
 
 def direction_callback(data):
+    global current_direction
     current_direction = data.data # sets the current direction variable to the updated value of the topic
-    rospy.loginfo("Direction: %s", current_direction) # write the information to the console
-    updated = True
+    #rospy.loginfo("Direction: %s", current_direction) # write the information to the console
+    status.updated = True
 
 def listen():
-    updated = False
     rospy.init_node('vision_subscriber', anonymous=True)
     rospy.Subscriber('direction', String, direction_callback)
     rospy.Subscriber('distance', Float32, distance_callback)
+
+    rate = rospy.Rate(10)
+
+    while not rospy.is_shutdown():
+        if status.updated: 
+            rospy.loginfo("MOVING FORWARD")
+            move(current_direction,current_distance)
+            status.updated = False # changed updated to false so that we do not move again based on old information
+            rate.sleep()
+        rate.sleep()
+
+
+
+
+
 
-    if updated: #only call the move function if we are working with new information 
-        move(direction,distance)
-        updated = False # changed updated to false so that we do not move again based on old information
 
 
-    rospy.spin()
 if __name__ == '__main__':
     listen()