diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..13566b81b018ad684f3a35fee301741b2734c8f4
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000000000000000000000000000000000000..105ce2da2d6447d11dfe32bfb846c3d5b199fc99
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+ <settings>
+ <option name="USE_PROJECT_PROFILE" value="false" />
+ <version value="1.0" />
+ </settings>
+</component>
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d1e22ecb89619a9c2dcf51a28d891a196d2462a0
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+ <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8" project-jdk-type="Python SDK" />
+</project>
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1a84fd2d494c8a0f4e685265fcdcbfd3d09c609d
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+ <component name="ProjectModuleManager">
+ <modules>
+ <module fileurl="file://$PROJECT_DIR$/.idea/ros2_project.iml" filepath="$PROJECT_DIR$/.idea/ros2_project.iml" />
+ </modules>
+ </component>
+</project>
\ No newline at end of file
diff --git a/.idea/ros2_project.iml b/.idea/ros2_project.iml
new file mode 100644
index 0000000000000000000000000000000000000000..68a356664d033f1eadb7bbdb60f24d922e313b8a
--- /dev/null
+++ b/.idea/ros2_project.iml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+ <component name="NewModuleRootManager">
+ <content url="file://$MODULE_DIR$" />
+ <orderEntry type="inheritedJdk" />
+ <orderEntry type="sourceFolder" forTests="false" />
+ </component>
+ <component name="TestRunnerService">
+ <option name="PROJECT_TEST_RUNNER" value="py.test" />
+ </component>
+</module>
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000000000000000000000000000000000000..35eb1ddfbbc029bcab630581847471d7f238ec53
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+ <component name="VcsDirectoryMappings">
+ <mapping directory="" vcs="Git" />
+ </component>
+</project>
\ No newline at end of file
diff --git a/tb3_challenge1.py b/tb3_challenge1.py
new file mode 100644
index 0000000000000000000000000000000000000000..274f0bce87ab303933fd1aa9c9bc2f38d4ff3829
--- /dev/null
+++ b/tb3_challenge1.py
@@ -0,0 +1,74 @@
+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 sys
+
+
+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.2
+
+ 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 scan_callback(self, msg):
+ """ is run whenever a LaserScan msg is received
+ """
+
+ if msg.ranges[0] > self.safe_distance:
+ self.vel(100, 0)
+ else:
+ self.vel(0, 0)
+ print("Wall found! Stopping..")
+ sys.exit()
+
+
+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()
\ No newline at end of file
diff --git a/tb3_challenge2.py b/tb3_challenge2.py
new file mode 100644
index 0000000000000000000000000000000000000000..c22485a77167a551a00113d202da3a8da6b4ccf2
--- /dev/null
+++ b/tb3_challenge2.py
@@ -0,0 +1,88 @@
+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 sys
+from time import sleep
+
+
+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.drive_allowed = True
+ self.turn_allowed = False
+
+ 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 scan_callback(self, msg):
+ """ is run whenever a LaserScan msg is received
+ """
+ if self.drive_allowed:
+ if msg.ranges[0] > self.safe_distance:
+ self.vel(80, 0)
+ else:
+ if msg.intensities[0] == 2.0 and not self.turn_allowed:
+ self.vel(0, 0)
+ print("Red wall found! \nTurning...")
+ self.vel(0, 85)
+ sleep(1)
+ self.vel(0, 0)
+ self.turn_allowed = True
+ pass
+ elif msg.intensities[0] != 2.0:
+ print("Wall found!")
+ print("Stopping..")
+ self.vel(0, 0)
+ self.drive_allowed = False
+ sys.exit()
+
+
+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()
\ No newline at end of file
diff --git a/tb3_challenge4.py b/tb3_challenge4.py
index beabfcf86c6418b5878d77a822ce74902c76d6a1..82908b0d763f096764259993cdf09c562e7a9bf4 100644
--- a/tb3_challenge4.py
+++ b/tb3_challenge4.py
@@ -5,10 +5,7 @@ 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
+from time import sleep, time
class Tb3(Node):
@@ -16,15 +13,15 @@ class Tb3(Node):
super().__init__('tb3')
self.cmd_vel_pub = self.create_publisher(
- Twist, # message type
- 'cmd_vel', # topic name
- 1) # history depth
+ 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
+ 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
@@ -32,7 +29,7 @@ class Tb3(Node):
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,33 +46,62 @@ 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
+
if not self.stop:
if not self.turn_allowed:
- if msg.ranges[0] > self.safe_distance or msg.ranges[0] == 0.0:
- print(msg.intensities[0])
- self.vel(50, 0)
+ for distance in front_area:
+ if distance > self.safe_distance or distance == 0.0:
+ self.vel(60, 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()
+
+ if self.turn_allowed:
+ if left < right:
+ print("Turning right!")
+ self.vel(0, -50)
+ sleep(0.25)
+ if self.check_to_stop_turn(msg):
+ sleep(0.1)
+ self.vel(0, 0)
+ self.turn_allowed = False
else:
- self.turn_allowed = True
- self.timestamp = time.time()
-
- if (time.time() - self.timestamp) <= 1 and self.turn_allowed:
- self.vel(0, 45)
- if (time.time() - self.timestamp) > 2 and self.turn_allowed:
- self.vel(0, 0)
- self.turn_allowed = False
+ print("Turning left!")
+ self.vel(0, 50)
+ sleep(0.25)
+ if self.check_to_stop_turn(msg):
+ 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.0 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):
@@ -95,4 +121,4 @@ def main(args=None):
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
diff --git a/tb3.py b/tb3_challenge5.py
similarity index 75%
rename from tb3.py
rename to tb3_challenge5.py
index d0c62b4cceda38913b9ae3d32616671d1ef4ecbc..2292108631c21197e96d3f2180f66ddd9edb69bc 100644
--- a/tb3.py
+++ b/tb3_challenge5.py
@@ -5,10 +5,7 @@ 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
+import sys
class Tb3(Node):
@@ -16,22 +13,22 @@ class Tb3(Node):
super().__init__('tb3')
self.cmd_vel_pub = self.create_publisher(
- Twist, # message type
- 'cmd_vel', # topic name
- 1) # history depth
+ 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
+ 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
-
+
def vel(self, lin_vel_percent, ang_vel_percent=0):
""" publishes linear and angular velocities in percent
"""
@@ -50,10 +47,10 @@ class Tb3(Node):
def drive(self, msg):
if not self.stop:
if self.check_if_goal(msg):
- self.vel(0, 0)
- self.stop = True
- print("Goal found!")
- return
+ self.vel(0, 0)
+ self.stop = True
+ print("Goal found!")
+ sys.exit()
if not self.turn_allowed:
if msg.ranges[0] > self.safe_distance:
self.vel(70, 0)
@@ -63,7 +60,7 @@ class Tb3(Node):
else:
if (msg.ranges[0] < self.safe_distance or msg.ranges[-40] < self.safe_distance + 0.05) \
- or msg.ranges[-90] < 0.2:
+ or msg.ranges[-90] < 0.2:
self.turn_left(msg)
print("Turning left")
elif msg.ranges[-70] > self.safe_distance + 0.1:
@@ -86,7 +83,7 @@ class Tb3(Node):
def turn_left(self, msg):
self.vel(0, 35)
-
+
def turn_right(self, msg):
self.vel(10, -35)
@@ -95,13 +92,6 @@ class Tb3(Node):
"""
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):
diff --git a/tb3_challenge5_old.py b/tb3_challenge5_old.py
deleted file mode 100644
index d92dd4593925e32d76cbd4af44a2d3a083900a09..0000000000000000000000000000000000000000
--- a/tb3_challenge5_old.py
+++ /dev/null
@@ -1,125 +0,0 @@
-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]
- front_area = msg.ranges[-40:40]
- left_white_distance = msg.ranges[40]
- right_white_distance = msg.ranges[-40]
-
- if not self.stop:
- if not self.turn_allowed:
- if (front > self.safe_distance or front == 0.0) \
- and (left_white_distance > self.safe_distance or left_white_distance == 0.0) \
- and (right_white_distance > self.safe_distance or right_white_distance == 0.0):
- self.vel(80, 0)
- else:
- # white
- 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, -80)
- if self.check_to_stop_turn(msg):
- self.vel(0, 0)
- self.turn_allowed = False
- else:
- self.vel(0, 80)
- if self.check_to_stop_turn(msg):
- self.vel(0, 0)
- self.turn_allowed = False
-
- def check_to_stop_turn(self, msg):
- if msg.ranges[0] > 1.2 and msg.ranges[90] > 0.2 and msg.ranges[-90] > 0.2 \
- and msg.ranges[40] > self.safe_distance and msg.ranges[-40] > self.safe_distance:
- 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()
\ No newline at end of file
diff --git a/tb3_dodge_in_area.py b/tb3_dodge_in_area.py
deleted file mode 100644
index 11045446d773b3fdacabe8f9e6b29085ee46f8d4..0000000000000000000000000000000000000000
--- a/tb3_dodge_in_area.py
+++ /dev/null
@@ -1,134 +0,0 @@
-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()
diff --git a/tb3_right_hand.py b/tb3_right_hand.py
deleted file mode 100644
index e6bd4585c111705fcccc8c8ab2e4b2474388c4c4..0000000000000000000000000000000000000000
--- a/tb3_right_hand.py
+++ /dev/null
@@ -1,141 +0,0 @@
-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.stop = False
- self.start = True
-
- 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):
- if not self.stop:
- if(self.start == True):
- if(msg.ranges[0]> 0.4):
- self.vel(90,0)
- else:
- if self.check_red_wall(msg):
- self.vel(0,0)
- self.stop = True
- self.start = False
- else:
- if(msg.ranges[0] < 0.4 or msg.ranges[-40]< 0.45):
- self.turn_left(msg)
- print('lefty')
- elif(msg.ranges[-60] > 0.45):
- self.turn_right(msg)
- print('right')
- elif(msg.ranges[-90] < 0.20):
- self.turn_left(msg)
- print('left')
- else:
- self.vel(50,0)
-
- def turn_right(self, msg):
- self.vel(0, 0)
- self.vel(0, -90)
- self.vel(0, 0)
-
- def turn_left(self, msg):
- self.vel(0, 0)
- self.vel(0, 90)
- self.vel(0, 0)
-
- def check_red_wall(self, msg):
- if msg.ranges[0] <= 0.4:
- if msg.intensities[0] == 2.0:
- return True
- if msg.ranges[90] <= 0.4:
- if msg.intensities[90] == 2.0:
- return True
- if msg.ranges[-90] <= 0.4:
- if msg.intensities[-90] == 2.0:
- return True
- if msg.ranges[180] <= 0.4:
- if msg.intensities[180] == 2.0:
- return True
- else:
- return 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()
\ No newline at end of file