Change files

tb3.py

@@ -31,7 +31,6 @@ class Tb3(Node):
         self.safe_distance = 0.4
         self.turn_allowed = False
         self.stop = False
-        self.timestamp = 0
     
     def vel(self, lin_vel_percent, ang_vel_percent=0):
         """ publishes linear and angular velocities in percent
@@ -49,56 +48,47 @@ class Tb3(Node):
         self.lin_vel_percent = lin_vel_percent
 
     def drive(self, msg):
-        front = msg.ranges[0]
-        left = msg.ranges[90]
-        right = msg.ranges[-90]
-        rear = msg.ranges[180]
-        left_area = msg.ranges[330:360]
-        right_area = msg.ranges[0:30]
-        front_area = left_area + right_area
-        left_white_distance = msg.ranges[40]
-        right_white_distance = msg.ranges[-40]
-
         if not self.stop:
+            if self.check_if_goal(msg):
+                    self.vel(0, 0)
+                    self.stop = True
+                    print("Goal found!")
+                    return
             if not self.turn_allowed:
-                for distance in front_area:
-                    if distance > self.safe_distance or distance == 0.0:
-                        self.vel(80, 0)
-                    else:
-                        # stop at red color
-                        if msg.intensities[0] == 2.0:
-                            self.vel(0, 0)
-                            self.stop = False
-                        else:
-                            self.turn_allowed = True
-                            self.timestamp = time.time()
-
-            if self.turn_allowed:
-                if left < right:
-                    self.vel(0, -60)
-                    if self.check_to_stop_turn(msg):
-                        print("Turning right!")
-                        time.sleep(0.1)
-                        self.vel(0, 0)
-                        self.turn_allowed = False
+                if msg.ranges[0] > self.safe_distance:
+                    self.vel(70, 0)
                 else:
-                    self.vel(0, 60)
-                    if self.check_to_stop_turn(msg):
-                        print("Turning left!")
-                        time.sleep(0.1)
-                        self.vel(0, 0)
-                        self.turn_allowed = False
-                
-    def check_to_stop_turn(self, msg):
-        left_area = msg.ranges[330:360]
-        right_area = msg.ranges[0:30]
-        front_area = left_area + right_area
-        for distance in front_area:    
-            if msg.ranges[0] > 1.2 and msg.ranges[90] > 0.2 and msg.ranges[-90] > 0.2 and distance > self.safe_distance:
-                print("Turn stopped!")
-                return True
+                    self.vel(0, 0)
+                    self.turn_allowed = True
+
             else:
-                return False
+                if (msg.ranges[0] < self.safe_distance or msg.ranges[-40] < self.safe_distance + 0.05) \
+                or msg.ranges[-90] < 0.2:
+                    self.turn_left(msg)
+                    print("Turning left")
+                elif msg.ranges[-70] > self.safe_distance + 0.1:
+                    self.turn_right(msg)
+                    print("Turning right")
+                else:
+                    self.vel(70, 0)
+
+    def check_if_goal(self, msg):
+        if msg.ranges[0] <= self.safe_distance and msg.intensities[0] == 2.0:
+            return True
+        elif msg.ranges[-90] <= self.safe_distance and msg.intensities[-90] == 2.0:
+            return True
+        elif msg.ranges[90] <= self.safe_distance and msg.intensities[90] == 2.0:
+            return True
+        elif msg.ranges[180] <= self.safe_distance and msg.intensities[180] == 2.0:
+            return True
+        else:
+            return False
+
+    def turn_left(self, msg):
+        self.vel(0, 35)
+    
+    def turn_right(self, msg):
+        self.vel(10, -35)
 
     def scan_callback(self, msg):
         """ is run whenever a LaserScan msg is received
@@ -131,4 +121,4 @@ def main(args=None):
 
 
 if __name__ == '__main__':
-    main()
\ No newline at end of file
+    main()

tb3_dodge_in_area.py

+import rclpy  # ROS client library
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+
+from sensor_msgs.msg import LaserScan
+from geometry_msgs.msg import Twist
+
+import time
+
+from datetime import datetime
+from datetime import timedelta
+
+
+class Tb3(Node):
+    def __init__(self):
+        super().__init__('tb3')
+
+        self.cmd_vel_pub = self.create_publisher(
+                Twist,      # message type
+                'cmd_vel',  # topic name
+                1)          # history depth
+
+        self.scan_sub = self.create_subscription(
+                LaserScan,
+                'scan',
+                self.scan_callback,  # function to run upon message arrival
+                qos_profile_sensor_data)  # allows packet loss
+
+        self.ang_vel_percent = 0
+        self.lin_vel_percent = 0
+        self.safe_distance = 0.4
+        self.turn_allowed = False
+        self.stop = False
+        self.timestamp = 0
+    
+    def vel(self, lin_vel_percent, ang_vel_percent=0):
+        """ publishes linear and angular velocities in percent
+        """
+        # for TB3 Waffle
+        MAX_LIN_VEL = 0.26  # m/s
+        MAX_ANG_VEL = 1.82  # rad/s
+
+        cmd_vel_msg = Twist()
+        cmd_vel_msg.linear.x = MAX_LIN_VEL * lin_vel_percent / 100
+        cmd_vel_msg.angular.z = MAX_ANG_VEL * ang_vel_percent / 100
+
+        self.cmd_vel_pub.publish(cmd_vel_msg)
+        self.ang_vel_percent = ang_vel_percent
+        self.lin_vel_percent = lin_vel_percent
+
+    def drive(self, msg):
+        front = msg.ranges[0]
+        left = msg.ranges[90]
+        right = msg.ranges[-90]
+        rear = msg.ranges[180]
+        left_area = msg.ranges[330:360]
+        right_area = msg.ranges[0:30]
+        front_area = left_area + right_area
+        left_white_distance = msg.ranges[40]
+        right_white_distance = msg.ranges[-40]
+
+        if not self.stop:
+            if not self.turn_allowed:
+                for distance in front_area:
+                    if distance > self.safe_distance or distance == 0.0:
+                        self.vel(80, 0)
+                    else:
+                        # stop at red color
+                        if msg.intensities[0] == 2.0:
+                            self.vel(0, 0)
+                            self.stop = False
+                        else:
+                            self.turn_allowed = True
+                            self.timestamp = time.time()
+
+            if self.turn_allowed:
+                if left < right:
+                    self.vel(0, -60)
+                    if self.check_to_stop_turn(msg):
+                        print("Turning right!")
+                        time.sleep(0.1)
+                        self.vel(0, 0)
+                        self.turn_allowed = False
+                else:
+                    self.vel(0, 60)
+                    if self.check_to_stop_turn(msg):
+                        print("Turning left!")
+                        time.sleep(0.1)
+                        self.vel(0, 0)
+                        self.turn_allowed = False
+                
+    def check_to_stop_turn(self, msg):
+        left_area = msg.ranges[330:360]
+        right_area = msg.ranges[0:30]
+        front_area = left_area + right_area
+        for distance in front_area:    
+            if msg.ranges[0] > 1.2 and msg.ranges[90] > 0.2 and msg.ranges[-90] > 0.2 and distance > self.safe_distance:
+                print("Turn stopped!")
+                return True
+            else:
+                return False
+
+    def scan_callback(self, msg):
+        """ is run whenever a LaserScan msg is received
+        """
+
+        self.drive(msg)
+        # self.detect_left_right(msg)
+
+#        print('Distances:')
+#        print('⬆️ :', msg.ranges[0], msg.intensities[0])
+#        print('⬇️ :', msg.ranges[180], msg.intensities[180])
+#        print('⬅️ :', msg.ranges[90], msg.intensities[90])
+#        print('➡️ :', msg.ranges[-90], msg.intensities[-90])
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    tb3 = Tb3()
+    print('waiting for messages...')
+
+    try:
+        rclpy.spin(tb3)  # Execute tb3 node
+        # Blocks until the executor (spin) cannot work
+    except KeyboardInterrupt:
+        pass
+
+    tb3.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()