More changes to lane demo

Robot_Development/catkin_ws/src/lane_demo/src/lane_demo.cpp

@@ -5,14 +5,11 @@
 #include "std_msgs/UInt64.h"
 #include "std_msgs/Float64.h"
 #include <math.h>
-#include <algorithm>
-#define LINEAR_SPEED 0.7
-#define ANGULAR_SPEED 1.75
-#define TURN_DURATION 500
-#define OBJECT_DIST_NEAR 30
-#define OBJECT_DIST_SAFE 60
+#define LINEAR_SPEED 0.65
+#define ANGULAR_SPEED 2
+#define OBJECT_DIST_NEAR 40
 
-enum STATE { FORWARD, STOP};
+enum STATE { FORWARD, TURN_RIGHT, TURN_LEFT, STOP};
 
 class SenseAndAvoid
 {
@@ -62,7 +59,7 @@ SenseAndAvoid::SenseAndAvoid()
   left_count = 0;
   right_count = 0;
   cur_error = 0;
-  Kp = .0155;
+  Kp = .015;
   Kv = 0;
   Kd = 0.035;
 }
@@ -73,6 +70,18 @@ void SenseAndAvoid::Lanes(const std_msgs::String::ConstPtr& msg)
   {
 	state = STOP;
   }
+  else if(msg->data.compare("Left lane does not exist") == 0)
+  {
+	state = TURN_RIGHT;
+  }
+  else if(msg->data.compare("Right lane does not exist") == 0)
+  {
+	state = TURN_LEFT;
+  }
+  else
+  {
+	state = FORWARD;
+  }
 }
 
 void SenseAndAvoid::Lane_Distance(const std_msgs::Float64::ConstPtr& msg)
@@ -102,31 +111,31 @@ void SenseAndAvoid::sonar_3Callback(const std_msgs::Int16::ConstPtr& msg)
 
 void SenseAndAvoid::sonarCallback(const std_msgs::Int16::ConstPtr& msg)
 {
-  if((msg->data < OBJECT_DIST_NEAR && msg->data > 0)) {
+  if((msg->data < OBJECT_DIST_NEAR && msg->data > 0) || (left_sonar < OBJECT_DIST_NEAR && left_sonar > 0) || (right_sonar < OBJECT_DIST_NEAR && right_sonar > 0)) {
     ROS_INFO("OBJECT IN THE WAY, STOP");
 	vel_msg.linear.x = 0;
     vel_msg.angular.z = 0;
     vel_pub.publish(vel_msg);
   }
-  else if(left_sonar < OBJECT_DIST_NEAR && left_sonar > 0)
+  else if(state == STOP)
   {
-	ROS_INFO("OBJECT IN THE WAY ON THE LEFT, STOP");
+	ROS_INFO("NO LANES, STOP");
 	vel_msg.linear.x = 0;
 	vel_msg.angular.z = 0;
 	vel_pub.publish(vel_msg);
   }
-  else if(right_sonar < OBJECT_DIST_NEAR && right_sonar > 0)
+  else if(state == TURN_LEFT)
   {
-	ROS_INFO("OBJECT IN THE WAY ON THE RIGHT, STOP");
+	ROS_INFO("TURNING LEFT");
 	vel_msg.linear.x = 0;
-    vel_msg.angular.z = 0;
+	vel_msg.angular.z = -ANGULAR_SPEED;
 	vel_pub.publish(vel_msg);
   }
-  else if(state == STOP)
+  else if(state == TURN_RIGHT)
   {
-	ROS_INFO("NO LANES, STOP");
+	ROS_INFO("TURNING_RIGHT");
 	vel_msg.linear.x = 0;
-	vel_msg.angular.z = 0;
+	vel_msg.angular.z = ANGULAR_SPEED;
 	vel_pub.publish(vel_msg);
   }
   else

Robot_Development/catkin_ws/src/lane_detection/src/detect.py

@@ -12,7 +12,7 @@ from std_msgs.msg import Float64
 pub = rospy.Publisher('lanes', String, queue_size=10)
 pub1 = rospy.Publisher('lane_distance', Float64, queue_size=10)
 rospy.init_node('lane_detection')
-rate = rospy.Rate(1000)  # 1000hz
+rate = rospy.Rate(500)  # 1000hz
 width = 325
 
 
@@ -57,7 +57,7 @@ def ROI(pic):
 # pic: original image to apply the pre-set region of interest too
 def ROI_real(pic):
     height = pic.shape[0]
-    triangle = np.array([[(0, height), (145, 300), (475, 300), (600, height)]], dtype=np.int32)
+    triangle = np.array([[(0, height), (200, 250), (400, 250), (600, height)]], dtype=np.int32)
     mask = np.zeros_like(pic)
     cv2.fillPoly(mask, triangle, 255)
     roi = cv2.bitwise_and(pic, mask)
@@ -210,7 +210,7 @@ def detectDeparture(left, car, right):
         left_lane = 1
 
     except IndexError:
-        print("Left lane does not exist")
+        return "Left lane does not exist"
     try:
         parametersRight = np.polyfit((right[0][0], right[0][-1]), (right[1][0], right[1][-1]), 1)
         rightEq = np.poly1d(parametersRight)
@@ -219,7 +219,7 @@ def detectDeparture(left, car, right):
         right_lane = 1
 
     except IndexError:
-        print("Right lane does not exist")
+        return "Right lane does not exist"
 
     if (left_lane == 0 and right_lane == 0):
         return "No Lanes"
@@ -244,6 +244,7 @@ def lane_status(lane_distance):
     rate.sleep()
 
 
+
 def lanes_detected(lanes):
     rospy.loginfo(lanes)
     pub.publish(lanes)
@@ -281,15 +282,15 @@ while not rospy.is_shutdown():
         Mpoints = find_middle(Lpoints, Rpoints)
 
         # (type(Rpoints[0][0]))
-        #plt.cla()
-        #plt.clf()
+        plt.cla()
+        plt.clf()
 
-        #plt.scatter(Rpoints[0], Rpoints[1])
-        #plt.scatter(Lpoints[0], Lpoints[1])
+        plt.scatter(Rpoints[0], Rpoints[1])
+        plt.scatter(Lpoints[0], Lpoints[1])
         lanes = detectDeparture(Lpoints, 400, Rpoints)
         lanes_detected(lanes)
         if (Mpoints != 0):
-            #plt.scatter(Mpoints[0], Mpoints[1])
+            plt.scatter(Mpoints[0], Mpoints[1])
             lane_distance = detectDepartureNew(Mpoints)
             lane_status(lane_distance)
 
@@ -297,9 +298,9 @@ while not rospy.is_shutdown():
             print("No midpoint calculated")
         #plt.scatter(310, 300)
 
-        #plt.imshow(frame, zorder=0)
+        plt.imshow(frame, zorder=0)
 
-        #plt.pause(.001)
+        plt.pause(.001)
 
     #lane = applyLines(frame, lines)
 
@@ -307,9 +308,8 @@ while not rospy.is_shutdown():
     # cv2.imshow("cropped", cropped)
     # cv2.imshow("frame", lane)
 
-    key = cv2.waitKey(1)
-    if key == 27:
-        break
-
-video.release()
-cv2.destroyAllWindows()
+    #key = cv2.waitKey(1)
+    #if key == 27:
+    #    break
+#video.release()
+#cv2.destroyAllWindows()