Skip to content
Snippets Groups Projects
Commit be448df2 authored by Patrick Simmel's avatar Patrick Simmel
Browse files

stuff no.2

parent 33e2e43d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 277 additions and 190 deletions
.idea/.gitignore 0 → 100644
+ 8
− 0
View file @ be448df2
# Default ignored files
/shelf/
/workspace.xml
# Editor-based HTTP Client requests
/httpRequests/
# Datasource local storage ignored files
/dataSources/
/dataSources.local.xml
.idea/inspectionProfiles/profiles_settings.xml 0 → 100644
+ 6
− 0
View file @ be448df2
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>
\ No newline at end of file
.idea/misc.xml 0 → 100644
+ 4
− 0
View file @ be448df2
<?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
.idea/modules.xml 0 → 100644
+ 8
− 0
View file @ be448df2
<?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
.idea/ros2_project.iml 0 → 100644
+ 11
− 0
View file @ be448df2
<?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
.idea/vcs.xml 0 → 100644
+ 6
− 0
View file @ be448df2
<?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
tb3_right_hand.py → tb3_challenge1.py
+ 74
− 0
View file @ be448df2
...@@ -5,10 +5,7 @@ from rclpy.qos import qos_profile_sensor_data ...@@ -5,10 +5,7 @@ from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
import time import sys
from datetime import datetime
from datetime import timedelta
class Tb3(Node): class Tb3(Node):
...@@ -16,22 +13,20 @@ class Tb3(Node): ...@@ -16,22 +13,20 @@ class Tb3(Node):
super().__init__('tb3') super().__init__('tb3')
self.cmd_vel_pub = self.create_publisher( self.cmd_vel_pub = self.create_publisher(
Twist, # message type Twist, # message type
'cmd_vel', # topic name 'cmd_vel', # topic name
1) # history depth 1) # history depth
self.scan_sub = self.create_subscription( self.scan_sub = self.create_subscription(
LaserScan, LaserScan,
'scan', 'scan',
self.scan_callback, # function to run upon message arrival self.scan_callback, # function to run upon message arrival
qos_profile_sensor_data) # allows packet loss qos_profile_sensor_data) # allows packet loss
self.ang_vel_percent = 0 self.ang_vel_percent = 0
self.lin_vel_percent = 0 self.lin_vel_percent = 0
self.safe_distance = 0.4 self.safe_distance = 0.2
self.stop = False
self.start = True
def vel(self, lin_vel_percent, ang_vel_percent=0): def vel(self, lin_vel_percent, ang_vel_percent=0):
""" publishes linear and angular velocities in percent """ publishes linear and angular velocities in percent
""" """
...@@ -47,78 +42,16 @@ class Tb3(Node): ...@@ -47,78 +42,16 @@ class Tb3(Node):
self.ang_vel_percent = ang_vel_percent self.ang_vel_percent = ang_vel_percent
self.lin_vel_percent = lin_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): def scan_callback(self, msg):
""" is run whenever a LaserScan msg is received """ is run whenever a LaserScan msg is received
""" """
self.drive(msg) if msg.ranges[0] > self.safe_distance:
# self.detect_left_right(msg) self.vel(100, 0)
else:
# print('Distances:') self.vel(0, 0)
# print('⬆️ :', msg.ranges[0], msg.intensities[0]) print("Wall found! Stopping..")
# print('⬇️ :', msg.ranges[180], msg.intensities[180]) sys.exit()
# print('⬅️ :', msg.ranges[90], msg.intensities[90])
# print('➡️ :', msg.ranges[-90], msg.intensities[-90])
def main(args=None): def main(args=None):
......
tb3_challenge5_old.py → tb3_challenge2.py
+ 88
− 0
View file @ be448df2
...@@ -5,10 +5,8 @@ from rclpy.qos import qos_profile_sensor_data ...@@ -5,10 +5,8 @@ from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
import time import sys
from time import sleep
from datetime import datetime
from datetime import timedelta
class Tb3(Node): class Tb3(Node):
...@@ -29,10 +27,9 @@ class Tb3(Node): ...@@ -29,10 +27,9 @@ class Tb3(Node):
self.ang_vel_percent = 0 self.ang_vel_percent = 0
self.lin_vel_percent = 0 self.lin_vel_percent = 0
self.safe_distance = 0.4 self.safe_distance = 0.4
self.drive_allowed = True
self.turn_allowed = False self.turn_allowed = False
self.stop = False
self.timestamp = 0
def vel(self, lin_vel_percent, ang_vel_percent=0): def vel(self, lin_vel_percent, ang_vel_percent=0):
""" publishes linear and angular velocities in percent """ publishes linear and angular velocities in percent
""" """
...@@ -48,61 +45,27 @@ class Tb3(Node): ...@@ -48,61 +45,27 @@ class Tb3(Node):
self.ang_vel_percent = ang_vel_percent self.ang_vel_percent = ang_vel_percent
self.lin_vel_percent = lin_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): def scan_callback(self, msg):
""" is run whenever a LaserScan msg is received """ is run whenever a LaserScan msg is received
""" """
if self.drive_allowed:
self.drive(msg) if msg.ranges[0] > self.safe_distance:
# self.detect_left_right(msg) self.vel(80, 0)
else:
print('Distances:') if msg.intensities[0] == 2.0 and not self.turn_allowed:
print('⬆️ :', msg.ranges[0], msg.intensities[0]) self.vel(0, 0)
print('⬇️ :', msg.ranges[180], msg.intensities[180]) print("Red wall found! \nTurning...")
print('⬅️ :', msg.ranges[90], msg.intensities[90]) self.vel(0, 85)
print('➡️ :', msg.ranges[-90], msg.intensities[-90]) 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): def main(args=None):
......
tb3_challenge4.py
+ 57
− 31
View file @ be448df2
...@@ -5,10 +5,7 @@ from rclpy.qos import qos_profile_sensor_data ...@@ -5,10 +5,7 @@ from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
import time from time import sleep, time
from datetime import datetime
from datetime import timedelta
class Tb3(Node): class Tb3(Node):
...@@ -16,15 +13,15 @@ class Tb3(Node): ...@@ -16,15 +13,15 @@ class Tb3(Node):
super().__init__('tb3') super().__init__('tb3')
self.cmd_vel_pub = self.create_publisher( self.cmd_vel_pub = self.create_publisher(
Twist, # message type Twist, # message type
'cmd_vel', # topic name 'cmd_vel', # topic name
1) # history depth 1) # history depth
self.scan_sub = self.create_subscription( self.scan_sub = self.create_subscription(
LaserScan, LaserScan,
'scan', 'scan',
self.scan_callback, # function to run upon message arrival self.scan_callback, # function to run upon message arrival
qos_profile_sensor_data) # allows packet loss qos_profile_sensor_data) # allows packet loss
self.ang_vel_percent = 0 self.ang_vel_percent = 0
self.lin_vel_percent = 0 self.lin_vel_percent = 0
...@@ -32,7 +29,7 @@ class Tb3(Node): ...@@ -32,7 +29,7 @@ class Tb3(Node):
self.turn_allowed = False self.turn_allowed = False
self.stop = False self.stop = False
self.timestamp = 0 self.timestamp = 0
def vel(self, lin_vel_percent, ang_vel_percent=0): def vel(self, lin_vel_percent, ang_vel_percent=0):
""" publishes linear and angular velocities in percent """ publishes linear and angular velocities in percent
""" """
...@@ -49,33 +46,62 @@ class Tb3(Node): ...@@ -49,33 +46,62 @@ class Tb3(Node):
self.lin_vel_percent = lin_vel_percent self.lin_vel_percent = lin_vel_percent
def drive(self, msg): 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.stop:
if not self.turn_allowed: if not self.turn_allowed:
if msg.ranges[0] > self.safe_distance or msg.ranges[0] == 0.0: for distance in front_area:
print(msg.intensities[0]) if distance > self.safe_distance or distance == 0.0:
self.vel(50, 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: else:
self.turn_allowed = True print("Turning left!")
self.timestamp = time.time() self.vel(0, 50)
sleep(0.25)
if (time.time() - self.timestamp) <= 1 and self.turn_allowed: if self.check_to_stop_turn(msg):
self.vel(0, 45) sleep(0.1)
if (time.time() - self.timestamp) > 2 and self.turn_allowed: self.vel(0, 0)
self.vel(0, 0) self.turn_allowed = False
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): def scan_callback(self, msg):
""" is run whenever a LaserScan msg is received """ is run whenever a LaserScan msg is received
""" """
self.drive(msg) 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): def main(args=None):
...@@ -95,4 +121,4 @@ def main(args=None): ...@@ -95,4 +121,4 @@ def main(args=None):
if __name__ == '__main__': if __name__ == '__main__':
main() main()
\ No newline at end of file
tb3.py → tb3_challenge5.py
+ 15
− 25
View file @ be448df2
...@@ -5,10 +5,7 @@ from rclpy.qos import qos_profile_sensor_data ...@@ -5,10 +5,7 @@ from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
import time import sys
from datetime import datetime
from datetime import timedelta
class Tb3(Node): class Tb3(Node):
...@@ -16,22 +13,22 @@ class Tb3(Node): ...@@ -16,22 +13,22 @@ class Tb3(Node):
super().__init__('tb3') super().__init__('tb3')
self.cmd_vel_pub = self.create_publisher( self.cmd_vel_pub = self.create_publisher(
Twist, # message type Twist, # message type
'cmd_vel', # topic name 'cmd_vel', # topic name
1) # history depth 1) # history depth
self.scan_sub = self.create_subscription( self.scan_sub = self.create_subscription(
LaserScan, LaserScan,
'scan', 'scan',
self.scan_callback, # function to run upon message arrival self.scan_callback, # function to run upon message arrival
qos_profile_sensor_data) # allows packet loss qos_profile_sensor_data) # allows packet loss
self.ang_vel_percent = 0 self.ang_vel_percent = 0
self.lin_vel_percent = 0 self.lin_vel_percent = 0
self.safe_distance = 0.4 self.safe_distance = 0.4
self.turn_allowed = False self.turn_allowed = False
self.stop = False self.stop = False
def vel(self, lin_vel_percent, ang_vel_percent=0): def vel(self, lin_vel_percent, ang_vel_percent=0):
""" publishes linear and angular velocities in percent """ publishes linear and angular velocities in percent
""" """
...@@ -50,10 +47,10 @@ class Tb3(Node): ...@@ -50,10 +47,10 @@ class Tb3(Node):
def drive(self, msg): def drive(self, msg):
if not self.stop: if not self.stop:
if self.check_if_goal(msg): if self.check_if_goal(msg):
self.vel(0, 0) self.vel(0, 0)
self.stop = True self.stop = True
print("Goal found!") print("Goal found!")
return sys.exit()
if not self.turn_allowed: if not self.turn_allowed:
if msg.ranges[0] > self.safe_distance: if msg.ranges[0] > self.safe_distance:
self.vel(70, 0) self.vel(70, 0)
...@@ -63,7 +60,7 @@ class Tb3(Node): ...@@ -63,7 +60,7 @@ class Tb3(Node):
else: else:
if (msg.ranges[0] < self.safe_distance or msg.ranges[-40] < self.safe_distance + 0.05) \ 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) self.turn_left(msg)
print("Turning left") print("Turning left")
elif msg.ranges[-70] > self.safe_distance + 0.1: elif msg.ranges[-70] > self.safe_distance + 0.1:
...@@ -86,7 +83,7 @@ class Tb3(Node): ...@@ -86,7 +83,7 @@ class Tb3(Node):
def turn_left(self, msg): def turn_left(self, msg):
self.vel(0, 35) self.vel(0, 35)
def turn_right(self, msg): def turn_right(self, msg):
self.vel(10, -35) self.vel(10, -35)
...@@ -95,13 +92,6 @@ class Tb3(Node): ...@@ -95,13 +92,6 @@ class Tb3(Node):
""" """
self.drive(msg) 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): def main(args=None):
......
tb3_dodge_in_area.py deleted 100644 → 0
+ 0
− 134
View file @ 33e2e43d
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()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment