def gbest_calculator():
global bot1_pbest, bot2_pbest,bot3_pbest,bot4_pbest,bot5_pbest, goal
if bot1_pbest and bot2_pbest and bot3_pbest and bot4_pbest and bot5_pbest:
l = 8.5
f1 = -np.exp(-(np.linalg.norm(np.array(goal)-np.array(bot1_pbest))**2)/l)
f2 = -np.exp(-(np.linalg.norm(np.array(goal)-np.array(bot2_pbest))**2)/l)
f3 = -np.exp(-(np.linalg.norm(np.array(goal)-np.array(bot3_pbest))**2)/l)
f4 = -np.exp(-(np.linalg.norm(np.array(goal)-np.array(bot4_pbest))**2)/l)
f5 = -np.exp(-(np.linalg.norm(np.array(goal)-np.array(bot5_pbest))**2)/l)
t = min(f1, f2, f3 ,f4, f5)
if t == f1:
g_gbest = bot1_pbest
if t == f2:
g_gbest = bot2_pbest
if t == f3:
g_gbest = bot3_pbest
if t == f4:
g_gbest = bot4_pbest
if t == f5:
g_gbest = bot5_pbest
print("gbest is here", g_gbest)
gbest_pub = rospy.Publisher('/gbest_calc', swarm, queue_size= 1)
update_gbest = swarm()
update_gbest.gbest = g_gbest
gbest_pub.publish(update_gbest)
rospy.sleep(5)
def main():
global ini_pos, vel, gbestl_1, domain
ini_pos = [
list(random.uniform(domain[0], domain[1]) for i in range(2))
for t in range(1)
]
vel = [[0, 0]]
start_pos = [-8, -2]
robot_id = 0
pso_run = pso_ros(robot_id)
print("ini_pos", ini_pos)
while not rospy.is_shutdown():
mypub = rospy.Publisher('/Bot_1/pbest', swarm, queue_size=1)
tmp_pos = ini_pos
tmp_vel = vel
bot1 = swarm()
bot1.header.stamp = rospy.Time.now()
bot1.header.frame_id = '/Bot-1'
bot1.pbest = tmp_pos[0]
mypub.publish(bot1)
get_gbest = rospy.Subscriber('/gbest_calc', swarm, callback_s)
print("gbestl", gbestl_1)
if gbestl_1:
print("am i violating swarm 1", gbestl_1)
new_pos, n_vel = pso_run.run_pso(tmp_pos, tmp_vel, gbestl_1)
laser_topic = '/robot_0/base_scan'
pose_topic = '/robot_0/odom'
pub_topic = '/robot_0/cmd_vel'
oa_obj = pso_movement(laser_topic, pose_topic, pub_topic,
start_pos, new_pos[0])
oa_obj.main()
if oa_obj.flag == 1:
ini_pos = new_pos
vel = n_vel
start_pos = new_pos[0]
# pflag = 0
else:
pass