def check_distance():
mr.init()
dist = distance()
while dist < 15:
print('Too close', dist)
mr.init()
mr.random_reorient()
te = temp()
hu = hum()
print("fall:", flower, "pressure:", p, "temperature:", t,
"humidity:", hu)
received = receive_data
check1 = True
check2 = True
try:
manual_message = received["Action"]
print("manual message", manual_message)
if manual_message == "manual_turn":
#######
init()
gpio.output(27, True)
gpio.output(22, False)
gpio.output(23, False)
gpio.output(24, True)
time.sleep(rotate_time)
gpio.output(27, False)
gpio.output(22, True)
gpio.output(23, True)
gpio.output(24, False)
time.sleep(0.025)
gpio.cleanup()
########
current_rotate_position = current_rotate_position + 1
print("current_rotate_position33", current_rotate_position)
def __init__(self):
motor.init()
running = False
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
sift = cv2.SIFT()
dice = [ image.open_image("%d.jpg" % (i,), i) for i in range(1, sides + 1) ]
[ image.process(sift, i) for i in dice ]
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks = 50)
flann = cv2.FlannBasedMatcher(index_params, search_params)
motor.init("/dev/ttyUSB0", 9600)
threading.Thread(target = shake_thread).start()
threading.Thread(target = grab_thread).start()
threading.Thread(target = display_thread).start()
cv_shake.acquire()
cv_shake.notify()
cv_shake.release()
num = [0 for i in range(sides) ]
nums = []
added = 0
log = open('log', 'a')
while running:
timeout=5)
except:
pass
rospy.loginfo("robot %s connect success", model_name.data)
return model_name.data
rospy.init_node('webots_robot_server', anonymous=True)
robot_global.robot_name = robot_connect()
laser.init()
for i in range(0, 3):
time_step.time_step_call()
motor.init()
motor.set_velocity(0, 0, 0, 0)
for i in range(0, 3):
time_step.time_step_call()
step_cnt = 0
while True:
motor.set_velocity(3, 3, 3, 3)
time_step.time_step_call()
time_step.time_step_call()
time_step.time_step_call()
laser_data, done = laser.get_laser_scan_data()
def drive(lcd):
try:
f = init_fp(lcd)
welcome_msg(lcd)
lcd.lcd_display_string("Smart DL: Push Black", 2)
lcd.lcd_display_string("QRCode: Push Red", 3)
drive_enable = False
while True:
button_state_dl = GPIO.input(17)
button_state_qr = GPIO.input(27)
if button_state_dl == GPIO.HIGH:
context, readers = init_smart_card_reader(lcd)
state = init_smart_card(lcd, context, readers)
lcd.lcd_display_string("Pls dont remove DL", 2)
lcd.lcd_display_string('Place your finger..', 3)
positionNumber, accuracyScore = verify_fp_from_sensor(lcd, f)
welcome_msg(lcd)
lcd.lcd_display_string("Pls dont remove DL", 2)
lcd.lcd_display_string('FingerPrint Matched-1', 3)
lcd.lcd_display_string('Verifying DL', 4)
sleep(3)
result = read_verify_dl(lcd, f, positionNumber)
if (result == 0):
welcome_msg(lcd)
lcd.lcd_display_string("FP Matched-1", 2)
lcd.lcd_display_string('DL check Fail', 3)
lcd.lcd_display_string('Pls insert valid DL', 4)
print("No match for DL")
state = wait_new_smart_card(lcd, context, readers, state)
#sleep(5)
execfile("ssc3.py")
else:
print('Found template at position #' + str(positionNumber))
print('The accuracy score is: ' + str(accuracyScore))
drive_enable = True
break
elif button_state_qr == GPIO.HIGH:
welcome_msg(lcd)
lcd.lcd_display_string('Place your finger..', 2)
positionNumber, accuracyScore = verify_fp_from_sensor(lcd, f)
welcome_msg(lcd)
lcd.lcd_display_string("FP - Matched", 2)
sleep(2)
#lcd.lcd_display_string("Show QR code", 2)
#sleep(1)
result = verify_qrcode(lcd)
if (result == False):
welcome_msg(lcd)
lcd.lcd_display_string("QR Code Match Fail", 2)
lcd.lcd_display_string("QR Code Attempts Over", 3)
sleep(10)
execfile(ssc3.py)
else:
print('QR code match')
drive_enable = True
break
if drive_enable == True:
welcome_msg(lcd)
lcd.lcd_display_string("Wishing Safe Driving", 2)
lcd.lcd_display_string("push red to stop", 3)
sleep(1)
motor.init()
pir = MotionSensor(23)
while True:
motor.right(1)
button_state_stop = GPIO.input(27)
if button_state_stop == GPIO.HIGH:
#motor.cleanup()
welcome_msg(lcd)
lcd.lcd_display_string("Car Stopped", 2)
lcd.lcd_display_string("Motion: Push Red", 3)
lcd.lcd_display_string("Exit: Push black", 4)
break
counter = 0
motion_detect = False
while True:
button_state_exit = GPIO.input(17)
button_state_pir = GPIO.input(27)
if button_state_exit == GPIO.HIGH:
welcome_msg(lcd)
lcd.lcd_display_string("Demo Over", 2)
exit(1)
elif button_state_pir == GPIO.HIGH or motion_detect:
if motion_detect == False:
welcome_msg(lcd)
lcd.lcd_display_string("Motion Detect Started", 2)
motion_detect = True
try:
with timeout(1, exception=RuntimeError):
pir.wait_for_motion()
counter = counter + 1
if counter > 2000:
lcd.lcd_display_string("SMS sent", 3)
gsm.sendsms(
'ALERT: Someone Locked in Car No: 1234')
pass
except RuntimeError:
counter = 0
pass
print("Motion detected!")
#sleep(2)
except Exception as e:
GPIO.setmode(GPIO.BCM)
print(e)
#motor.cleanup()
GPIO.cleanup()
error_msg(lcd, e)
def __init__(self, laser_dim, collision_threshold):
self.my_case = -1
global global_rn
global_rn = random.random()
self.laser_dim = laser_dim
self.collision_threshold = collision_threshold
rospy.init_node('webots_env', anonymous=True)
robot_global.robot_name = robot_connect()
human.human_name = human_connect()
robot_global.PubSetPositionReq = rospy.Publisher(
'/simulation_set_position_req', Vector3, queue_size=1)
robot_global.SubSetPositionRes = rospy.Subscriber(
'/simulation_set_position_res', Bool, set_position_res_callback)
###new add for human_pose
human.PubSetPositionReq = rospy.Publisher(
'/simulation_set_human_pose_req', Vector3, queue_size=1)
human.SubSetPositionRes = rospy.Subscriber(
'/simulation_set_human_pose_res', Bool,
set_human_pose_res_callback)
# human.PubSetRotationReq = rospy.Publisher('/simulation_set_rotation_req', Quaternion, queue_size=1)
#human.SubSetRotationRes = rospy.Subscriber('/simulation_set_rotation_res', Bool,
# set_rotation_res_callback)
robot_global.PubSetRotationReq = rospy.Publisher(
'/simulation_set_rotation_req', Quaternion, queue_size=1)
robot_global.SubSetRotationRes = rospy.Subscriber(
'/simulation_set_rotation_res', Bool, set_rotation_res_callback)
robot_global.PubResetNodePhsicsReq = rospy.Publisher(
'/simulation_reset_node_physics_req', Bool, queue_size=1)
robot_global.SubResetNodePhsicsRes = rospy.Subscriber(
'/simulation_reset_node_physics_res', Bool,
reset_node_physics_res_callback)
robot_global.PubGetPositionReq = rospy.Publisher(
'/simulation_get_position_req', Bool, queue_size=1)
robot_global.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_position_res', Vector3, get_position_res_callback)
robot_global.PubGetRotationReq = rospy.Publisher(
'/simulation_get_rotation_req', Bool, queue_size=1)
robot_global.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_rotation_res', Quaternion,
get_rotation_res_callback)
human.PubGetPositionReq = rospy.Publisher(
'/simulation_get_human_position_req', Bool, queue_size=1)
human.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_human_position_res', Vector3,
get_human_position_res_callback)
human.PubGetRotationReq = rospy.Publisher(
'/simulation_get_human_rotation_req', Bool, queue_size=1)
human.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_human_rotation_res', Quaternion,
get_human_rotation_res_callback)
for i in range(0, 5):
time_step.time_step_call() #zuoyong? hjk
motor.init()
motor.set_velocity(0, 0, 0, 0)
time_step.time_step_call()
time_step.time_step_call()
time_step.time_step_call()
laser.init()
#laser.get_laser_scan_data()
self.reward_range = (-np.inf, np.inf)
self.action_history1 = 0
self.action_history2 = 0
self.action_history3 = 0
for i in range(0, 5):
time_step.time_step_call()
#get robot pose
pub_get_position_req()
while robot_global.get_position_res_flag is False:
time_step.time_step_call()
pub_get_rotation_req()
while robot_global.get_rotation_res_flag is False:
time_step.time_step_call()
#get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
rpos, rrot = getopposite(human.position, human.rotation, self.my_case)
dist1 = getDisXZ(robot_global.position.x, robot_global.position.z,
robot_global.rotation.w, rpos.x, rpos.z, rrot.w)
rtang, rot1, rot2 = getanglefea(robot_global.position.x,
robot_global.position.z,
robot_global.rotation.w, rpos.x,
rpos.z, rrot.w)
self.distp = dist1 #past distance
self.rot1p = rot1
self.rot2p = rot2
self.init_dist_pos = 0
self.wintimes_dist_key = 10
self.wintimes_all = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.wintimes_win = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.wintimes = 0
self.collisiontimes = 0
self.N = int(6.6 * unit + 10)
self.M = int(9.9 * unit + 10)
self.mp = self.init_map_hjktest()
self.lenp = 0
self.old_robot_postion = copy.deepcopy(robot_global.position)
self.old_robot_rotation = copy.deepcopy(robot_global.rotation)
def __init__(self, laser_dim, collision_threshold):
self.laser_dim = laser_dim
self.collision_threshold = collision_threshold
rospy.init_node('webots_env', anonymous=True)
robot_global.robot_name = robot_connect()
human.human_name = human_connect()
robot_global.PubSetPositionReq = rospy.Publisher(
'/simulation_set_position_req', Vector3, queue_size=1)
robot_global.SubSetPositionRes = rospy.Subscriber(
'/simulation_set_position_res', Bool, set_position_res_callback)
robot_global.PubSetRotationReq = rospy.Publisher(
'/simulation_set_rotation_req', Quaternion, queue_size=1)
robot_global.SubSetRotationRes = rospy.Subscriber(
'/simulation_set_rotation_res', Bool, set_rotation_res_callback)
robot_global.PubResetNodePhsicsReq = rospy.Publisher(
'/simulation_reset_node_physics_req', Bool, queue_size=1)
robot_global.SubResetNodePhsicsRes = rospy.Subscriber(
'/simulation_reset_node_physics_res', Bool,
reset_node_physics_res_callback)
robot_global.PubGetPositionReq = rospy.Publisher(
'/simulation_get_position_req', Bool, queue_size=1)
robot_global.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_position_res', Vector3, get_position_res_callback)
robot_global.PubGetRotationReq = rospy.Publisher(
'/simulation_get_rotation_req', Bool, queue_size=1)
robot_global.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_rotation_res', Quaternion,
get_rotation_res_callback)
human.PubGetPositionReq = rospy.Publisher(
'/simulation_get_human_position_req', Bool, queue_size=1)
human.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_human_position_res', Vector3,
get_human_position_res_callback)
human.PubGetRotationReq = rospy.Publisher(
'/simulation_get_human_rotation_req', Bool, queue_size=1)
human.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_human_rotation_res', Quaternion,
get_human_rotation_res_callback)
for i in range(0, 5):
time_step.time_step_call()
motor.init()
motor.set_velocity(0, 0, 0, 0)
time_step.time_step_call()
time_step.time_step_call()
time_step.time_step_call()
laser.init()
#laser.get_laser_scan_data()
self.reward_range = (-np.inf, np.inf)
self.action_history1 = 0
self.action_history2 = 0
self.action_history3 = 0
self.goal_x = 0
self.goal_z = 0
self.dis_x = None
self.dis_z = None
self.dis_x_old = None
self.dis_z_old = None
#goal state
self.goalstate = 0
for i in range(0, 5):
time_step.time_step_call()
#get robot pose
pub_get_position_req()
while robot_global.get_position_res_flag is False:
time_step.time_step_call()
pub_get_rotation_req()
while robot_global.get_rotation_res_flag is False:
time_step.time_step_call()
#get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()