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 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 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 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 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 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(): 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 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()
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()
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 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 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()
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()