diff --git a/Robot_Development/catkin_ws/build/lane_demo/CMakeFiles/lane_demo.dir/src/lane_demo.cpp.o b/Robot_Development/catkin_ws/build/lane_demo/CMakeFiles/lane_demo.dir/src/lane_demo.cpp.o
index cd4ef97474de24d5972d8016fab8c8258f4023fa..fa052af17275d9d722c33084742fb4c84b3d7f04 100644
Binary files a/Robot_Development/catkin_ws/build/lane_demo/CMakeFiles/lane_demo.dir/src/lane_demo.cpp.o and b/Robot_Development/catkin_ws/build/lane_demo/CMakeFiles/lane_demo.dir/src/lane_demo.cpp.o differ
diff --git a/Robot_Development/catkin_ws/devel/lib/lane_demo/lane_demo b/Robot_Development/catkin_ws/devel/lib/lane_demo/lane_demo
index e4acd1e95d972fcbd00223c0c3a2e336d7221f39..1740c35bd12a74a363d8fa41c19f64b0359451d7 100755
Binary files a/Robot_Development/catkin_ws/devel/lib/lane_demo/lane_demo and b/Robot_Development/catkin_ws/devel/lib/lane_demo/lane_demo differ
diff --git a/Robot_Development/catkin_ws/src/lane_demo/src/lane_demo.cpp b/Robot_Development/catkin_ws/src/lane_demo/src/lane_demo.cpp
index 11a587a982f835a13100bc2dcea79ab92cabfca4..cca2760c7c477103e8cccec447ba0ae60b200bbc 100644
--- a/Robot_Development/catkin_ws/src/lane_demo/src/lane_demo.cpp
+++ b/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;
}
@@ -71,7 +68,19 @@ void SenseAndAvoid::Lanes(const std_msgs::String::ConstPtr& msg)
{
if(msg->data.compare("No Lanes") == 0)
{
- state = STOP;
+ 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;
}
}
@@ -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);
+ 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_pub.publish(vel_msg);
+ 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
diff --git a/Robot_Development/catkin_ws/src/lane_detection/src/detect.py b/Robot_Development/catkin_ws/src/lane_detection/src/detect.py
index 2e91052e09df65a133671cd54a76717cde18c038..688e87ec179c9738139ba24062b7421253c57ad6 100755
--- a/Robot_Development/catkin_ws/src/lane_detection/src/detect.py
+++ b/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,35 +282,34 @@ 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)
else:
print("No midpoint calculated")
- # plt.scatter(310, 300)
+ #plt.scatter(310, 300)
- #plt.imshow(frame, zorder=0)
+ plt.imshow(frame, zorder=0)
- #plt.pause(.001)
+ plt.pause(.001)
- # lane = applyLines(frame, lines)
+ #lane = applyLines(frame, lines)
# cv2.imshow("edges", wEdges)
# 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()