Turn function

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "C:/KODE/2_semester_kode/Computer_teknologi_projekt/askjaer_turtlebot3/turtlebot3/turtlebot3"
+}

turtlebot3_example/nodes/turtlebot3_obstacle

@@ -21,26 +21,31 @@ import rospy
 import math
 from sensor_msgs.msg import LaserScan
 from geometry_msgs.msg import Twist
+import time  # time module
 
 LINEAR_VEL = 0.22
-STOP_DISTANCE = 0.2
+STOP_DISTANCE = 0.50
 LIDAR_ERROR = 0.05
+ANGLE = 90
 SAFE_STOP_DISTANCE = STOP_DISTANCE + LIDAR_ERROR
 
+
 class Obstacle():
+
     def __init__(self):
         self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
         self.obstacle()
-        
+
     def get_scan(self):
         scan = rospy.wait_for_message('scan', LaserScan)
         scan_filter = []
-
-        samples = len(scan.ranges)  # The number of samples is defined in 
-                                    # turtlebot3_<model>.gazebo.xacro file,
-                                    # the default is 360.
-        samples_view = 1            # 1 <= samples_view <= samples
-
+
+        samples = len(scan.ranges)  # The number of samples is defined in
+        # turtlebot3_<model>.gazebo.xacro file,
+        # the default is 360.
+
+        samples_view = ANGLE        # 1 <= samples_view <= samples
+
         if samples_view > samples:
             samples_view = samples
 
@@ -50,40 +55,93 @@ class Obstacle():
         else:
             left_lidar_samples_ranges = -(samples_view//2 + samples_view % 2)
             right_lidar_samples_ranges = samples_view//2
-            
+
             left_lidar_samples = scan.ranges[left_lidar_samples_ranges:]
             right_lidar_samples = scan.ranges[:right_lidar_samples_ranges]
             scan_filter.extend(left_lidar_samples + right_lidar_samples)
 
         for i in range(samples_view):
+            # set value to 3.5 in case of a faulty reading and hope we don't get all 0 readings
             if scan_filter[i] == float('Inf'):
                 scan_filter[i] = 3.5
             elif math.isnan(scan_filter[i]):
-                scan_filter[i] = 0
-
+                scan_filter[i] = 3.5
+            elif scan_filter[i] == 0:
+                scan_filter[i] = 3.5
+
         return scan_filter
+
+
 
     def obstacle(self):
         twist = Twist()
         turtlebot_moving = True
+        t_end = time.time() + 60  # time_time returns time since 1st jan 1970
+        speed_updates = 0  # for average speed
+        speed_accumulation = 0  # for average speed
+        collision_count = 0  # collision counter
+
+        #function for turning
+        def turn(min_left, min_right, turn_value):
+         if min_right < min_left:
+             twist.angular.z = turn_value  # left
+             self._cmd_pub.publish(twist)
+             rospy.loginfo('Turn left!')
+         else:  # min_distance_left < min_distance_right
+             twist.angular.z = -turn_value  # right
+             self._cmd_pub.publish(twist)
+             rospy.loginfo('Turn right!')
 
-        while not rospy.is_shutdown():
+        #runtime loopet
+        while time.time() < t_end:  # runs for 60 secs
             lidar_distances = self.get_scan()
             min_distance = min(lidar_distances)
+            min_distance_right = min(lidar_distances[0:29])
+            min_distance_center = min(lidar_distances[30:59])
+            min_distance_left = min(lidar_distances[60:89])
 
-            if min_distance < SAFE_STOP_DISTANCE:
-                if turtlebot_moving:
-                    twist.linear.x = 0.0
-                    twist.angular.z = 0.0
+            if min_distance_center < SAFE_STOP_DISTANCE:  # something need to be happen
+                if min_distance_center > 0.40:  # dist between 55 and 35
+                    rospy.loginfo('Between 55 and 40')
+                    turn(min_distance_left, min_distance_right, 1)
+
+                elif min_distance_center > 0.25:  # dist between 35 and 20
+                    rospy.loginfo('Between 40 and 25')
+                    if min_distance_right < min_distance_left:
+                        # twist.linear.x *= 0.5
+                        twist.angular.z = 2  # left
+                        self._cmd_pub.publish(twist)
+                        rospy.loginfo('Turn left!')
+                    else:  # min_distance_left < min_distance_right
+                        # twist.linear.x *= 0.5
+                        twist.angular.z = -2  # right
+                        self._cmd_pub.publish(twist)
+                        rospy.loginfo('Turn right!')
+                else:
+                    rospy.loginfo('Below 20')
+                    twist.linear.x = 0
+                    twist.angular.z = 5  # left
                     self._cmd_pub.publish(twist)
-                    turtlebot_moving = False
-                    rospy.loginfo('Stop!')
+                    rospy.loginfo('Turn left!')
+
             else:
                 twist.linear.x = LINEAR_VEL
                 twist.angular.z = 0.0
                 self._cmd_pub.publish(twist)
                 turtlebot_moving = True
-                rospy.loginfo('Distance of the obstacle : %f', min_distance)
+                rospy.loginfo('Distance of the obstacle : %f',
+                              min_distance_center)
+
+            # collision
+            if min_distance_center < 0.15 or min_distance_left < 0.15 or min_distance_right < 0.15:
+                collision_count = collision_count + 1
+
+            speed_updates = speed_updates + 1
+            speed_accumulation = speed_accumulation + twist.linear.x
+
+        rospy.loginfo('average speed : %f', speed_accumulation / speed_updates)
+        rospy.loginfo('collision count : %f', collision_count)
+
 
 def main():
     rospy.init_node('turtlebot3_obstacle')
@@ -92,5 +150,6 @@ def main():
     except rospy.ROSInterruptException:
         pass
 
+
 if __name__ == '__main__':
     main()