diff --git a/src/my_pkg/my_pkg/__pycache__/tb3.cpython-310.pyc b/src/my_pkg/my_pkg/__pycache__/tb3.cpython-310.pyc
index 71e26e84ce9dc90c1b04c09be90a9eff769278cf..c95266396129645f6060d1e651819583cfb2f4e3 100644
Binary files a/src/my_pkg/my_pkg/__pycache__/tb3.cpython-310.pyc and b/src/my_pkg/my_pkg/__pycache__/tb3.cpython-310.pyc differ
diff --git a/tb3.py b/tb3.py
index 15c0cfbc142d020fcebaffece54ebb27a5454658..d0c62b4cceda38913b9ae3d32616671d1ef4ecbc 100644
--- a/tb3.py
+++ b/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()
diff --git a/tb3_dodge_in_area.py b/tb3_dodge_in_area.py
new file mode 100644
index 0000000000000000000000000000000000000000..11045446d773b3fdacabe8f9e6b29085ee46f8d4
--- /dev/null
+++ b/tb3_dodge_in_area.py
@@ -0,0 +1,134 @@
+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()