def subsomption_architecture(nb):
rospy.init_node('subsomption_architecture', anonymous=True)
v = Channel()
v.activated = False
v.speed_left = 0
v.speed_right = 0
# Le noeud publie des ordres de deplacement a destination de simu_fastsim :
pub_l = rospy.Publisher('/simu_fastsim/speed_left', Float32, queue_size=10)
pub_r = rospy.Publisher('/simu_fastsim/speed_right',
Float32,
queue_size=10)
for n in range(nb):
rospy.Subscriber("/subsomption/channel" + str(n), Channel,
CallBack_module_cl(n))
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
speed_l = 0
speed_r = 0
for i in range(nb - 1, -1, -1):
if (channel[i].activated):
rospy.loginfo(rospy.get_caller_id() + " module actif: %d", i)
speed_l = channel[i].speed_left
speed_r = channel[i].speed_right
break
for i in range(nb):
channel[i] = v
pub_l.publish(speed_l)
pub_r.publish(speed_r)
r.sleep()
def subsomption_architecture(nb):
rospy.init_node('subsomption_architecture', anonymous=True)
v=Channel()
v.activated=False
v.speed_left=0
v.speed_right=0
pub_l = rospy.Publisher('/simu_fastsim/speed_left', Float32 , queue_size=10)
pub_r = rospy.Publisher('/simu_fastsim/speed_right', Float32 , queue_size=10)
for n in range(nb):
rospy.Subscriber("/subsomption/channel"+str(n), Channel, CallBack_module_cl(n))
r = rospy.Rate(10) # 10hz
speed_l=0
speed_r=0
while not rospy.is_shutdown():
#speed_l=0
#speed_r=0
for i in range(nb-1,-1,-1):
if (channel[i].activated):
rospy.loginfo(rospy.get_caller_id()+" module actif: %d",i)
speed_l=channel[i].speed_left
speed_r=channel[i].speed_right
break
for i in range(nb):
channel[i]=v
pub_l.publish(speed_l)
pub_r.publish(speed_r)
r.sleep()
def my_behavior():
global bumper_r
global bumper_l
global count_backward
global count_turn
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel1', Channel , queue_size=10)
r = rospy.Rate(10) # 10hz
v=Channel()
v.activated=True
v.speed_left=0
v.speed_right=0
count_backward=0
count=0
max_threshold = 80
med_threshold = 70
min_threshold = 60
speed = .1
speed_avoid = 0.3
while not rospy.is_shutdown():
# print(" ".join([str(int(x)) for x in lasers.ranges]))
angle = 0
if (lasers.ranges):
laser_min = min(lasers.ranges)
laser_index = lasers.ranges.index(laser_min)
laser_angle = lasers.angle_min + laser_index * lasers.angle_increment
# print("min, index", laser_min, laser_index, laser_angle)
if laser_min < min_threshold:
x = min_threshold / laser_min
v.speed_left = speed + speed_avoid * x * np.sign(laser_angle)
v.speed_right = speed - speed_avoid * x * np.sign(laser_angle)
pub.publish(v)
if max_threshold > laser_min > med_threshold:
x = 1 # laser_min / max_threshold
v.speed_left = speed - speed_avoid * x * np.sign(laser_angle)
v.speed_right = speed + speed_avoid * x * np.sign(laser_angle)
pub.publish(v)
# compute the value of v that will be sent to the subsomption channel.
r.sleep()
def my_behavior():
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel0', Channel , queue_size=10)
r = rospy.Rate(10) # 10hz
v=Channel()
v.activated=True
v.speed_left=speed
v.speed_right=-speed
count=0
while not rospy.is_shutdown():
# compute the value of v that will be sent to the subsomption channel.
pub.publish(v)
r.sleep()
def avancer_tout_droit():
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
#rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
#rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel0', Channel, queue_size=10)
r = rospy.Rate(10) # 10hz
v = Channel()
v.activated = True
v.speed_left = 1
v.speed_right = 1
count = 0
while not rospy.is_shutdown():
# compute the value of v that will be sent to the subsomption channel.
pub.publish(v)
r.sleep()
def my_behavior():
global timer
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
#rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
# rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
# rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel3', Channel, queue_size=10)
r = rospy.Rate(10) # 10hz
v = Channel()
while not rospy.is_shutdown():
v.activated = False
if (timer == 0):
timer = 10
if ((10 * random.random()) < 2 or timer < 10):
v.activated = True
v.speed_left = 0.5
v.speed_right = 1
timer -= 1
count = 0
# compute the value of v that will be sent to the subsomption channel.
pub.publish(v)
r.sleep()
def my_behavior():
global lasers, seuil
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
# rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
# rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel1', Channel, queue_size=10)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
obstaclesRight = []
obstaclesLeft = []
laserData = lasers.ranges
for i in range(0, len(laserData)):
if (laserData[i] > seuil):
if (i < len(laserData) / 2):
obstaclesLeft.append(laserData[i])
else:
obstaclesRight.append(laserData[i])
v = Channel()
v.activated = False
if (len(obstaclesRight) != 0 or len(obstaclesLeft) != 0):
v.activated = True
minObstL = 200
minObstR = 200
v.speed_left = 1
v.speed_right = 0
if (len(obstaclesRight) != 0):
minObstR = min(obstaclesRight)
if (len(obstaclesLeft) != 0):
minObstL = min(obstaclesLeft)
if (minObstR < minObstL):
v.speed_left = 0
v.speed_right = 1
count = 0
# compute the value of v that will be sent to the subsomption channel.
pub.publish(v)
r.sleep()
def my_behavior():
global timer
global bumped
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
#rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel5', Channel , queue_size=10)
r = rospy.Rate(10) # 10hz
v=Channel()
while not rospy.is_shutdown():
if (timer==0):
v.activated=False
timer=50
bumped=False
else:
if (bumped):
v.activated=True
if (timer<20):
v.speed_left=0
v.speed_right=1
rospy.loginfo(" ROTATE")
else:
v.speed_left=-1
v.speed_right=-1
rospy.loginfo(" BACK")
timer=timer-1
count=0
# compute the value of v that will be sent to the subsomption channel.
pub.publish(v)
r.sleep()
def strategy_gating(nbCh,gatingType):
rospy.init_node('strategy_gating', anonymous=True)
v=Channel()
v.activated=False
v.speed_left=0
v.speed_right=0
# Parameters of State building
th_neglectedWall = 35
angleLMin = 0
angleLMax = 55
angleFMin=56
angleFMax=143
angleRMin=144
angleRMax=199
# The node publishes movement orders for simu_fastsim :
pub_l = rospy.Publisher('/simu_fastsim/speed_left', Float32 , queue_size=10)
pub_r = rospy.Publisher('/simu_fastsim/speed_right', Float32 , queue_size=10)
# The node receives order suggestions by the behavioral modules (channels):
for n in range(nbCh):
rospy.Subscriber("/navigation_strategies/channel"+str(n), Channel, CallBack_module_cl(n))
# If necessary, the node receives sensory information from simu_fastsim:
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/radars", Int16MultiArray, callback_radar)
rospy.Subscriber("/simu_fastsim/odom", Odometry, callback_odom)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# Targetted operating frequency of the node:
r = rospy.Rate(10) # 10hz
# Q-learning related stuff
# definition of states at time t and t-1
S_t = ''
S_tm1 = ''
Q = {}
# start time and timing related things
startT = rospy.get_time()
rospy.loginfo("Start time"+str(startT))
trial = 0
nbTrials = 60
trialDuration = np.zeros((nbTrials))
choice = -1
rew = 0
i2strat = ['wall follower','guidance']
# Main loop:
while (not rospy.is_shutdown()) and (trial <nbTrials):
speed_l=0
speed_r=0
# processing of the sensory data :
#------------------------------------------------
# 1) has the robot found the reward ?
#rospy.loginfo("pose: "+str(odom.pose.pose.position.x)+", "+str(odom.pose.pose.position.y))
dist2goal = math.sqrt((odom.pose.pose.position.x-goalx)**2+(odom.pose.pose.position.y-goaly)**2)
#rospy.loginfo(dist2goal)
# if so, teleport it:
if (dist2goal<30):
rospy.wait_for_service('simu_fastsim/teleport')
try:
# teleport robot
teleport = rospy.ServiceProxy('simu_fastsim/teleport', Teleport)
x = 300 #20+random.randrange(520)
y = 40
th = random.randrange(360)/2*math.pi
resp1 = teleport(x, y, th)
# store information about the duration of the finishing trial:
currT = rospy.get_time()
trialDuration[trial] = currT - startT
startT = currT
rospy.loginfo("Trial "+str(trial)+" duration:"+str(trialDuration[trial]))
trial +=1
rew = 1
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
except rospy.ServiceException, e:
print "Service call failed: %s"%e
# 2) has the robot bumped into a wall ?
#rospy.loginfo("BUMPERS "+str(bumper_r)+' '+str(bumper_l))
if bumper_r or bumper_l:
rew = -1
#rospy.loginfo("BING! A wall...")
# 3) build the state, that will be used by learning, from the sensory data
#rospy.loginfo("Nb laser scans="+str(len(lasers.ranges)))
if len(lasers.ranges) == 200:
S_tm1 = S_t
S_t = ''
# determine if obstacle on the left:
wall='0'
for l in lasers.ranges[angleLMin:angleLMax]:
if l < th_neglectedWall:
wall ='1'
S_t += wall
# determine if obstacle in front:
wall='0'
for l in lasers.ranges[angleFMin:angleFMax]:
#rospy.loginfo("front:"+str(l))
if l < th_neglectedWall:
wall ='1'
S_t += wall
# determine if obstacle in front:
wall='0'
for l in lasers.ranges[angleRMin:angleRMax]:
if l < th_neglectedWall:
wall ='1'
S_t += wall
# check if we are receiving radar measurements
if radar != 0:
radar_list = []
for i in range(len(radar)):
radar_list.append(radar[i])
#rospy.loginfo(str(radar_list))
S_t += str(radar_list[0])
#rospy.loginfo("S(t)="+S_t+" ; S(t-1)="+S_tm1)
# The chosen gating strategy is to be coded here:
#------------------------------------------------
if gatingType=='random':
choice = random.randrange(nbCh)
#choice = 1
rospy.loginfo("Module actif: "+i2strat[choice])
speed_l=channel[choice].speed_left
speed_r=channel[choice].speed_right
#------------------------------------------------
elif gatingType=='randomPersist':
# a choice is made every 5 steps
rospy.loginfo("randomPersist: arbitrage a coder.")
pass
#------------------------------------------------
elif gatingType=='guidance':
choice = 1
rospy.loginfo("Module actif: "+i2strat[choice])
speed_l=channel[choice].speed_left
speed_r=channel[choice].speed_right
#------------------------------------------------
elif gatingType=='wallFollower':
choice = 0
rospy.loginfo("Module actif: "+i2strat[choice])
speed_l=channel[choice].speed_left
speed_r=channel[choice].speed_right
#------------------------------------------------
elif gatingType=='qlearning':
rospy.loginfo("qlearning: arbitrage a coder.")
pass
#------------------------------------------------
else:
rospy.loginfo(gatingType+' unknown.')
exit()
#for i in range(nbCh):
# channel[i]=v
pub_l.publish(speed_l)
pub_r.publish(speed_r)
r.sleep()
#for i in range(nbCh):
# channel[i]=v
pub_l.publish(speed_l)
pub_r.publish(speed_r)
r.sleep()
# Log files opening
logDuration = open('DureesEssais_a_'+str(alpha)+'_b_'+str(beta)+'_g_'+str(gamma)+'_'+str(startT),'w')
for i in range(nbTrials):
rospy.loginfo('T = '+str(trialDuration[i]))
logDuration.write(str(i)+' '+str(trialDuration[i])+'\n')
logDuration.close()
#-------------------------------------------
if __name__ == '__main__':
nbch=int(sys.argv[1])
gatingType=sys.argv[2]
v=Channel()
v.activated=False
v.speed_left=0
v.speed_right=0
channel=[v for i in range(nbch)]
try:
strategy_gating(nbch,gatingType)
except rospy.ROSInterruptException: pass
def radarGuidance():
rospy.init_node('radarGuidance', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/radars", Int16MultiArray, callback_radars)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/navigation_strategies/channel1', Channel , queue_size=10)
r = rospy.Rate(freq) # 10hz
# behavioral parameters:
v_fwd = 1.5 * ratio # @CHANGED
v_turn = 0.75 * ratio # @CHANGED
v=Channel()
v.activated=False
v.speed_left = 0
v.speed_right = 0
# scans used to check wall distances:
angleLMin = 0
angleLMax = 55
angleFMin = 56
angleFMax = 143
angleRMin = 144 #199-55
angleRMax = 199
th_obstacleTooClose = 13
while not rospy.is_shutdown():
# compute the value of v that will be sent to the subsomption channel.
wallTooCloseL = False
wallTooCloseF = False
wallTooCloseR = False
v.speed_left = 0
v.speed_right = 0
# check if we are really receiving laser scan measurements in accordance with the robot settings
if len(lasers.ranges) == 200:
# determine if obstacle too close:
for i in range(len(lasers.ranges)):
#rospy.loginfo("front:"+str(l))
if lasers.ranges[i] < th_obstacleTooClose:
if i in range(angleLMin,angleLMax):
wallTooCloseL = True
if i in range(angleFMin,angleFMax):
wallTooCloseF = True
if i in range(angleRMin,angleRMax):
wallTooCloseR = True
# check if we are receiving radar measurements
if radars != 0:
radars_list = []
for i in range(len(radars)):
radars_list.append(radars[i])
#rospy.loginfo(str(radars_list))
# if the goal i in front of the robot :
if wallTooCloseF:
# rospy.loginfo('WALL F')
v.speed_left = -v_fwd
v.speed_right = -v_fwd
elif bumper_r or wallTooCloseR:
# rospy.loginfo('WALL R')
v.speed_left = v_fwd
v.speed_right = -v_fwd
elif bumper_l or wallTooCloseL:
# rospy.loginfo('WALL L')
v.speed_left = -v_fwd
v.speed_right = v_fwd
elif (7 in radars_list) :
# rospy.loginfo('FWD L')
v.speed_left = v_fwd
v.speed_right = v_fwd*.95
elif (0 in radars_list):
# rospy.loginfo('FWD R')
v.speed_left = v_fwd*.95
v.speed_right = v_fwd
# if it is on the left :
elif (6 in radars_list) or (5 in radars_list):
# rospy.loginfo('LEFT')
v.speed_left = v_fwd
v.speed_right = v_turn
# if it is on the right :
elif (1 in radars_list) or (2 in radars_list):
# rospy.loginfo('RIGHT')
v.speed_left = v_turn
v.speed_right = v_fwd
# if it is behind :
elif (3 in radars_list) :
# rospy.loginfo('BEHIND R')
v.speed_left = -v_fwd
v.speed_right = v_fwd
elif (4 in radars_list) :
# rospy.loginfo('BEHIND L')
v.speed_left = v_fwd
v.speed_right = -v_fwd
# publish the suggested movement
pub.publish(v)
r.sleep()
def suivi_murs():
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
#rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
#rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel2', Channel, queue_size=10)
r = rospy.Rate(10) # 10hz
v = Channel()
v.activated = False
v.speed_left = -1
v.speed_right = -1
count = 0
timer_bumper = 0 #added
flag_right = False
flag_left = False
while not rospy.is_shutdown():
# compute the value of v that will be sent to the subsomption channel.
ranges = np.array(lasers.ranges)
minrange = +1000
if ranges.size > 0:
exteriorlasersleft = ranges[0:int(ranges.size * 0.3)]
exteriorlasersright = ranges[int(ranges.size * 0.7):ranges.size *
1]
"""
seuilmur = 0.5
validmurgauche = ranges[0] - ranges[2] > seuilmur
validmurdroite = ranges[ranges.size-1] - ranges[ranges.size-3] > seuilmur
"""
minexteriorlaserleft = np.amin(exteriorlasersleft)
minexteriorlaserright = np.amin(exteriorlasersright)
centerlasers = ranges[int(ranges.size *
0.3):int(ranges.size *
0.7)] #20% central
mincenterlaser = np.amin(centerlasers)
#print(minrange , np.argmin(ranges), "/", ranges.size)
seuilcentre = 50
seuilcote = 40
seuilmax = 100 #pour se rapprocher du mur si on s'en eloigne sans avoir d'obstacle
speed = 0.2
print(minexteriorlaserleft, mincenterlaser, minexteriorlaserright)
#print(validmurgauche, validmurdroite)
if mincenterlaser < seuilcentre or minexteriorlaserleft < seuilcote or minexteriorlaserright < seuilcote: # or not (validmurgauche or validmurdroite): #mur trop proche = desactive ce comportement
print("DESACTIVE")
v.activated = False
flag_left = False
flag_right = False
elif minexteriorlaserleft < seuilmax and not flag_right: #suit mur a gauche
print("SUIVI DE MUR GAUCHE")
v.speed_left = +1
v.speed_right = +1
flag_left = True
flag_right = False
v.activated = True
elif flag_left and minexteriorlaserleft > seuilmax: #s'eloigne trop
print("RAPPROCHE MUR GAUCHE")
#tourne a gauche un peu
v.speed_left = +speed
v.speed_right = -speed
elif minexteriorlaserright < seuilmax and not flag_left: #suit mur a droite
print("SUIVI DE MUR DROIT")
v.speed_left = +1
v.speed_right = +1
flag_right = True
flag_left = False
v.activated = True
elif flag_right and minexteriorlaserright > seuilmax: #s'eloigne trop
print("RAPPROCHE MUR DROIT")
#tourne a droite un peu
v.speed_left = -speed
v.speed_right = +speed
else:
print("ERROR")
v.activated = False
flag_right = False
flag_left = False
else:
print("RANGES SIZE == 0")
v.activated = False
flag_right = False
flag_left = False
"""
if flag_bumper and timer_bumper<10:
v.speed_left=-1
v.speed_right=-1
v.activated=True
timer_bumper += 1
elif flag_bumper and timer_bumper<15: #a ajuster
v.speed_left=+1
v.speed_right=-1
v.activated=True
timer_bumper += 1
elif flag_bumper:
v.activated=False
timer_bumper = 0
flag_lasers= False
"""
#publish v
pub.publish(v)
r.sleep()
def strategy_gating(nbCh, gatingType):
rospy.init_node('strategy_gating', anonymous=True)
v = Channel()
v.activated = False
v.speed_left = 0
v.speed_right = 0
# Parameters of State building
th_neglectedWall = 35
angleLMin = 0
angleLMax = 55
angleFMin = 56
angleFMax = 143
angleRMin = 144
angleRMax = 199
# The node publishes movement orders for simu_fastsim :
pub_l = rospy.Publisher('/simu_fastsim/speed_left', Float32, queue_size=10)
pub_r = rospy.Publisher('/simu_fastsim/speed_right',
Float32,
queue_size=10)
# The node receives order suggestions by the behavioral modules (channels):
for n in range(nbCh):
rospy.Subscriber("/navigation_strategies/channel" + str(n), Channel,
CallBack_module_cl(n))
# If necessary, the node receives sensory information from simu_fastsim:
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/radars", Int16MultiArray, callback_radar)
rospy.Subscriber("/simu_fastsim/odom", Odometry, callback_odom)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# Targetted operating frequency of the node:
r = rospy.Rate(freq) # 10hz
# Q-learning related stuff
# definition of states at time t and t-1
S_t = ''
S_tm1 = ''
Q = {}
# start time and timing related things
startT = rospy.get_time()
rospy.loginfo("Start time" + str(startT))
trial = 0
nbTrials = 60 # @CHANGED
trialDuration = np.zeros((nbTrials))
choice = -1
rew = 0
i2strat = ['wall follower', 'guidance']
# --------------------- initialisation de varialbes ---------------------- #
# @CHANGED
time_of_change = None
Q = dict([(str(a) + str(b) + str(c) + str(d), [0, 0]) for a in range(2)
for b in range(2) for c in range(2) for d in range(8)])
walls = [0 for i in range(nbTrials)]
# ------------------------------------------------------------------------ #
# Main loop:
while (not rospy.is_shutdown()) and (trial < nbTrials):
speed_l = 0
speed_r = 0
# processing of the sensory data :
#------------------------------------------------
# 1) has the robot found the reward ?
#rospy.loginfo("pose: "+str(odom.pose.pose.position.x)+", "+str(odom.pose.pose.position.y))
dist2goal = math.sqrt((odom.pose.pose.position.x - goalx)**2 +
(odom.pose.pose.position.y - goaly)**2)
#rospy.loginfo(dist2goal)
# if so, teleport it:
if (
dist2goal < 30
) and rospy.get_time() - startT > 1: # @CHANGED : on vérifie que la
# simulation tourne depuis plus d'1 seconde
# pour éviter un bug
rospy.wait_for_service('simu_fastsim/teleport')
try:
# teleport robot
teleport = rospy.ServiceProxy('simu_fastsim/teleport',
Teleport)
x = 300 #20+random.randrange(520)
y = 40
th = random.randrange(360) / 2 * math.pi
resp1 = teleport(x, y, th)
# store information about the duration of the finishing trial:
currT = rospy.get_time()
trialDuration[trial] = currT - startT
startT = currT
rospy.loginfo("Trial " + str(trial) + " duration:" +
str(trialDuration[trial]))
trial += 1
rew = 1
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# 2) has the robot bumped into a wall ?
#rospy.loginfo("BUMPERS "+str(bumper_r)+' '+str(bumper_l))
if bumper_r or bumper_l:
rew = -1
#rospy.loginfo("BING! A wall...")
# 3) build the state, that will be used by learning, from the sensory data
#rospy.loginfo("Nb laser scans="+str(len(lasers.ranges)))
if len(lasers.ranges) == 200:
S_tm1 = S_t
S_t = ''
# determine if obstacle on the left:
wall = '0'
for l in lasers.ranges[angleLMin:angleLMax]:
if l < th_neglectedWall:
wall = '1'
S_t += wall
# determine if obstacle in front:
wall = '0'
for l in lasers.ranges[angleFMin:angleFMax]:
#rospy.loginfo("front:"+str(l))
if l < th_neglectedWall:
wall = '1'
S_t += wall
# determine if obstacle in front:
wall = '0'
for l in lasers.ranges[angleRMin:angleRMax]:
if l < th_neglectedWall:
wall = '1'
S_t += wall
# check if we are receiving radar measurements
if radar != 0:
radar_list = []
for i in range(len(radar)):
radar_list.append(radar[i])
#rospy.loginfo(str(radar_list))
S_t += str(radar_list[0])
#rospy.loginfo("S(t)="+S_t+" ; S(t-1)="+S_tm1)
# The chosen gating strategy is to be coded here:
#------------------------------------------------
if gatingType == 'random':
choice = random.randrange(nbCh)
#choice = 1
# rospy.loginfo("Module actif: "+i2strat[choice])
speed_l = channel[choice].speed_left
speed_r = channel[choice].speed_right
#------------------------------------------------
elif gatingType == 'randomPersist':
# a choice is made every 5 steps
if time_of_change is None or time.time(
) - time_of_change >= time_step:
time_of_change = time.time()
choice = random.randrange(nbCh)
# rospy.loginfo("randomPersist: arbitrage a coder.")
# rospy.loginfo("Module actif: "+i2strat[choice])
speed_l = channel[choice].speed_left
speed_r = channel[choice].speed_right
#------------------------------------------------
elif gatingType == 'guidance':
choice = 1
# rospy.loginfo("Module actif: "+i2strat[choice])
speed_l = channel[choice].speed_left
speed_r = channel[choice].speed_right
#------------------------------------------------
elif gatingType == 'wallFollower':
choice = 0
# rospy.loginfo("Module actif: "+i2strat[choice])
speed_l = channel[choice].speed_left
speed_r = channel[choice].speed_right
# ----------------------- stratégie Qlearning ------------------------ #
# @CHANGED
elif gatingType == 'qlearning':
new_choice = time_of_change is None or time.time(
) - time_of_change >= time_step or S_t != S_tm1
# mise à jour de la Q-table
if (new_choice or rew != 0) and S_t != '' and S_tm1 != '':
if rew == -1:
walls[trial] += 1
d_t = rew + gamma * max(Q[S_t]) - Q[S_tm1][choice]
Q[S_tm1][choice] += alpha * d_t
rew = 0 # ne plus apprendre de cette récompense
# nouveau choix
if new_choice:
time_of_change = time.time()
if S_t: #dodge the null case
p0 = softmax(Q[S_t], 0)
choice = 0 if random.random() < p0 else 1
else:
choice = random.randrange(nbCh)
# print(sum([any([v != 0 for v in x]) for x in Q.values()]))
speed_l = channel[choice].speed_left
speed_r = channel[choice].speed_right
# -------------------------------------------------------------------- #
else:
rospy.loginfo(gatingType + ' unknown.')
exit()
#for i in range(nbCh):
# channel[i]=v
pub_l.publish(speed_l)
pub_r.publish(speed_r)
r.sleep()
speed_l=0
speed_r=0
while not rospy.is_shutdown():
#speed_l=0
#speed_r=0
for i in range(nb-1,-1,-1):
if (channel[i].activated):
rospy.loginfo(rospy.get_caller_id()+" module actif: %d",i)
speed_l=channel[i].speed_left
speed_r=channel[i].speed_right
break
for i in range(nb):
channel[i]=v
pub_l.publish(speed_l)
pub_r.publish(speed_r)
r.sleep()
if __name__ == '__main__':
nbch=int(sys.argv[1])
v=Channel()
v.activated=False
v.speed_left=0
v.speed_right=0
channel=[v for i in range(nbch)]
try:
subsomption_architecture(nbch)
except rospy.ROSInterruptException: pass
def wallFollower():
rospy.init_node('wallFollower', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/navigation_strategies/channel0',
Channel,
queue_size=10)
r = rospy.Rate(freq) # 10hz
# behavioral parameters:
lMaxRange = 3000
robotRadius = 10 #15
th_obstacleFront = 30 #35
th_wallTooClose = 20 #25
th_wallTooFar = 30 #35
th_neglectedWall = 40 #50
v_fwd = 1 * ratio # @CHANGED
v_turn = 0.9 * ratio # @CHANGED
v_turnf = 0.5 * ratio # @CHANGED
v = Channel()
v.activated = False
v.speed_left = 0
v.speed_right = 0
# computation of the scans used to check front obstacle:
angleFrontMin = 99 - int(atan2(robotRadius, th_obstacleFront) / pi * 180)
angleFrontMax = 100 + int(atan2(robotRadius, th_obstacleFront) / pi * 180)
# scans used to check wall distances:
angleLMin = 0
angleLMax = 55
angleRMin = 199 - 55
angleRMax = 199
rospy.loginfo("Wall Follower: Angles " + str(angleFrontMin) + " " +
str(angleFrontMax) + " " + str(angleLMin) + " " +
str(angleLMax) + " " + str(angleRMin) + " " + str(angleRMax))
lastWallOnLeft = True
while not rospy.is_shutdown():
# compute the value of v that will be sent to the subsomption channel.
v.speed_left = 0
v.speed_right = 0
obstacleFront = False
wallTooCloseL = False
wallTooFarL = False
wallOKL = False
wallOKR = False
wallTooCloseR = False
wallTooFarR = False
distFrontMin = lMaxRange
# check if we are really receiving laser scan measurements in accordance with the robot settings
if len(lasers.ranges) == 200:
# determine if obstacle in front:
for l in lasers.ranges[angleFrontMin:angleFrontMax]:
#rospy.loginfo("front:"+str(l))
if l < distFrontMin:
distFrontMin = l
if l < th_obstacleFront:
obstacleFront = True
# determine if walls are within the "too close" and the "too far" L & R bands:
distWallLMin = lMaxRange
distWallRMin = lMaxRange
for i in range(angleLMin, angleLMax):
if lasers.ranges[i] < distWallLMin:
distWallLMin = lasers.ranges[i]
if lasers.ranges[i] * cos(
(10 - i) / 180. * pi) < th_wallTooClose:
#rospy.loginfo("Too close L("+str(i)+"):"+str(lasers.ranges[i])+" "+str(cos((10-i)/180.*pi))+" "+str(lasers.ranges[i]*cos((10-i)/180.*pi)))
wallTooCloseL = True
elif lasers.ranges[i] * cos(
(10 - i) / 180. * pi) < th_wallTooFar:
wallOKL = True
elif lasers.ranges[i] * cos(
(10 - i) / 180. * pi) < th_neglectedWall:
wallTooFarL = True
for i in range(angleRMin, angleRMax):
if lasers.ranges[i] < distWallRMin:
distWallRMin = lasers.ranges[i]
if lasers.ranges[i] * cos(
(189 - i) / 180. * pi) < th_wallTooClose:
#rospy.loginfo("Too close L("+str(i)+"):"+str(lasers.ranges[i])+" "+str(cos((10-i)/180.*pi))+" "+str(lasers.ranges[i]*cos((10-i)/180.*pi)))
wallTooCloseR = True
elif lasers.ranges[i] * cos(
(189 - i) / 180. * pi) < th_wallTooFar:
wallOKR = True
elif lasers.ranges[i] * cos(
(189 - i) / 180. * pi) < th_neglectedWall:
wallTooFarR = True
#rospy.loginfo("Wall min distances, L:"+str(distWallLMin)+" R:"+str(distWallRMin))
#rospy.loginfo("Walls L, too close:"+str(wallTooCloseL)+" too far:"+str(wallTooFarL)+" R, too close:"+str(wallTooCloseR)+" too far:"+str(wallTooFarR))
# Choose policy based on front obstacle and lateral walls detection:
if obstacleFront:
if lastWallOnLeft:
v.speed_left = -v_turn
v.speed_right = v_turn
else:
v.speed_left = v_turn
v.speed_right = -v_turn
# rospy.loginfo("Wall Follower: OBSTACLE - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooCloseL and (not (wallTooCloseR)
or distWallLMin < distWallRMin):
lastWallOnLeft = True
v.speed_left = v_turn
v.speed_right = v_fwd
#v.speed_left= v_fwd
#v.speed_right= v_turn
# rospy.loginfo("Wall Follower: L TOO CLOSE - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooCloseR and (not (wallTooCloseL)
or distWallLMin > distWallRMin):
lastWallOnLeft = False
v.speed_left = v_fwd
v.speed_right = v_turn
#v.speed_left= v_turn
#v.speed_right= v_fwd
# rospy.loginfo("Wall Follower: R TOO CLOSE - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallOKL:
lastWallOnLeft = True
v.speed_left = v_fwd
v.speed_right = v_fwd
# rospy.loginfo("Wall Follower: L OK - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallOKR:
lastWallOnLeft = False
v.speed_left = v_fwd
v.speed_right = v_fwd
# rospy.loginfo("Wall Follower: R OK - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooFarL and (not (wallTooFarR)
or distWallLMin < distWallRMin):
lastWallOnLeft = True
v.speed_left = v_fwd
v.speed_right = v_turn
#v.speed_left= v_turn
#v.speed_right= v_fwd
# rospy.loginfo("Wall Follower: L TOO FAR - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooFarR and (not (wallTooFarL)
or distWallLMin > distWallRMin):
lastWallOnLeft = False
v.speed_left = v_turn
v.speed_right = v_fwd
#v.speed_left= v_fwd
#v.speed_right= v_turn
# rospy.loginfo("Wall Follower: R TOO FAR - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif lastWallOnLeft:
v.speed_left = v_fwd
v.speed_right = v_turnf
# rospy.loginfo("Wall Follower: LOST WALL, L - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
else:
v.speed_left = v_turnf
v.speed_right = v_fwd
# rospy.loginfo("Wall Follower: LOST WALL, R - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
'''
# OLD CODE, BEFORE FASTSIM UPDATE
# Choose policy based on front obstacle and lateral walls detection:
if obstacleFront:
if lastWallOnLeft:
v.speed_left= -v_turn
v.speed_right= v_turn
else:
v.speed_left= v_turn
v.speed_right= -v_turn
rospy.loginfo("Wall Follower: OBSTACLE - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooCloseL and (not(wallTooCloseR) or distWallLMin<distWallRMin):
lastWallOnLeft = True
v.speed_left= v_turn
v.speed_right= v_fwd
#v.speed_left= v_fwd
#v.speed_right= v_turn
rospy.loginfo("Wall Follower: L TOO CLOSE - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooCloseR and (not(wallTooCloseL) or distWallLMin>distWallRMin):
lastWallOnLeft = False
v.speed_left= v_fwd
v.speed_right= v_turn
#v.speed_left= v_turn
#v.speed_right= v_fwd
rospy.loginfo("Wall Follower: R TOO CLOSE - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallOKL :
lastWallOnLeft = True
v.speed_left= v_fwd
v.speed_right= v_fwd
rospy.loginfo("Wall Follower: L OK - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallOKR :
lastWallOnLeft = False
v.speed_left= v_fwd
v.speed_right= v_fwd
rospy.loginfo("Wall Follower: R OK - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooFarL and (not(wallTooFarR) or distWallLMin<distWallRMin):
lastWallOnLeft = True
v.speed_left= v_fwd
v.speed_right= v_turn
#v.speed_left= v_turn
#v.speed_right= v_fwd
rospy.loginfo("Wall Follower: L TOO FAR - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif wallTooFarR and (not(wallTooFarL) or distWallLMin>distWallRMin):
lastWallOnLeft = False
v.speed_left= v_turn
v.speed_right= v_fwd
#v.speed_left= v_fwd
#v.speed_right= v_turn
rospy.loginfo("Wall Follower: R TOO FAR - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
elif lastWallOnLeft:
v.speed_left= 2
v.speed_right= 1
rospy.loginfo("Wall Follower: LOST WALL, L - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
else:
v.speed_left= 1
v.speed_right= 2
rospy.loginfo("Wall Follower: LOST WALL, R - Speed L:"+str(v.speed_left)+" R:"+str(v.speed_right))
'''
# publish the suggested movement
pub.publish(v)
r.sleep()
def my_behavior():
global bumper_r
global bumper_l
global count_backward
global count_turn
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel2', Channel, queue_size=10)
r = rospy.Rate(10) # 10hz
v = Channel()
v.activated = True
v.speed_left = 0
v.speed_right = 0
count_backward = 0
count = 0
while not rospy.is_shutdown():
if (bumper_l != 0 or bumper_r != 0):
count_backward = 5
v.activated = True
if (count_backward > 0):
v.speed_left = -1
v.speed_right = -1
count_backward -= 1
if (count_backward == 0):
count_turn = 11
pub.publish(v)
elif (count_turn > 0):
v.speed_left = -1
v.speed_right = 1
count_turn -= 1
pub.publish(v)
else:
v.speed_left = 0
v.speed_right = 0
v.activated = False
# compute the value of v that will be sent to the subsomption channel.
r.sleep()
def bumpers_demitour():
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
#rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel3', Channel , queue_size=10)
r = rospy.Rate(10) # 10hz
v=Channel()
v.activated=False
v.speed_left=-1
v.speed_right=-1
count=0
timer_bumper=0 #added
flag_bumper = False
while not rospy.is_shutdown():
# compute the value of v that will b e sent to the subsomption channel.
if (bumper_r or bumper_l):
flag_bumper = True
if flag_bumper and timer_bumper<10:
v.speed_left=-1
v.speed_right=-1
v.activated=True
timer_bumper += 1
elif flag_bumper and timer_bumper<15: #a ajuster
v.speed_left=+1
v.speed_right=-1
v.activated=True
timer_bumper += 1
elif flag_bumper:
v.activated=False
timer_bumper = 0
flag_bumper= False
#publish v
pub.publish(v)
r.sleep()
def evite_collision():
rospy.init_node('my_behavior', anonymous=True)
# remove the subscriptions you don't need.
rospy.Subscriber("/simu_fastsim/laser_scan", LaserScan, callback_lasers)
#rospy.Subscriber("/simu_fastsim/right_bumper", Bool, callback_right_bumper)
#rospy.Subscriber("/simu_fastsim/left_bumper", Bool, callback_left_bumper)
# replace /subsomption/channel0 by /subsomption/channeli where i is the number of the channel you want to publish in
pub = rospy.Publisher('/subsomption/channel1', Channel, queue_size=10)
r = rospy.Rate(10) # 10hz
v = Channel()
v.activated = False
v.speed_left = -1
v.speed_right = -1
count = 0
timer_bumper = 0 #added
flag_right = False
flag_left = False
while not rospy.is_shutdown():
# compute the value of v that will be sent to the subsomption channel.
ranges = np.array(lasers.ranges)
minrange = +1000
if ranges.size > 0:
minrange = np.amin(ranges)
#print(minrange , np.argmin(ranges), "/", ranges.size)
if minrange < 50:
v.activated = True
print(">>>", minrange)
speed = 0.5
if np.argmin(ranges) < (ranges.size //
2) and not flag_left: #mur left
v.speed_left = -speed
v.speed_right = +speed
flag_right = True
flag_left = False
elif not flag_right: #mur right
v.speed_left = +speed
v.speed_right = -speed
flag_left = True
flag_right = False
else:
v.activated = False
flag_right = False
flag_left = False
else:
v.activated = False
flag_right = False
flag_left = False
"""
if flag_bumper and timer_bumper<10:
v.speed_left=-1
v.speed_right=-1
v.activated=True
timer_bumper += 1
elif flag_bumper and timer_bumper<15: #a ajuster
v.speed_left=+1
v.speed_right=-1
v.activated=True
timer_bumper += 1
elif flag_bumper:
v.activated=False
timer_bumper = 0
flag_lasers= False
"""
#publish v
pub.publish(v)
r.sleep()
