def cb_hrvo(self, event):
self.update_all()
v_des = compute_V_des(self.position, self.goal, self.v_max)
self.velocity = RVO_update(self.position, v_des, self.velocity_detect,
self.ws_model)
#print("position",self.position)
#print("velocity",self.velocity)
info = [[0, 0] for i in range(4)]
for i in range(4):
dis, angle = self.process_ang_dis(self.velocity[i][0],
self.velocity[i][1], self.yaw[i])
#self.dis_pid[i].update(dis)
self.angu_pid[i].update(angle)
#dis_out = max(min(self.dis_pid[i].output,1),-1) * -1
dis_out = max(min(dis, 1), -1)
ang_out = max(min(self.angu_pid[i].output, 1), -1)
#print(i,dis_out,ang_out)
self.cmd_drive[i] = self.control_cmd_drive(dis_out, ang_out)
info[i][0] = self.cmd_drive[i].left
info[i][1] = self.cmd_drive[i].right
#print(info)
self.pub_v1.publish(self.cmd_drive[0])
self.pub_v2.publish(self.cmd_drive[1])
self.pub_v3.publish(self.cmd_drive[2])
self.pub_v4.publish(self.cmd_drive[3])
def cb_hrvo(self, event):
epoch = 0
iteration = 60
record = np.zeros([iteration])
for i in range(iteration):
rospy.sleep(0.5)
while True:
self.update_all()
v_des = compute_V_des(self.position, self.goal, self.v_max)
self.velocity = RVO_update(
self.position, v_des, self.velocity_detect, self.ws_model)
for i in range(4):
dis, angle = self.process_ang_dis(
self.velocity[i][0], self.velocity[i][1], self.yaw[i])
cmd = Twist()
cmd.linear.x = dis * 0.4
cmd.angular.z = angle * 0.4
self.cmd_drive[i] = cmd
self.pub_v1.publish(self.cmd_drive[0])
self.pub_v2.publish(self.cmd_drive[1])
self.pub_v3.publish(self.cmd_drive[2])
self.pub_v4.publish(self.cmd_drive[3])
min_dis, done = self.check_state()
# print min_dis
if min_dis < 1.7:
self.random_init()
record[epoch] = 0
break
if done:
self.random_init()
record[epoch] = 1
break
print epoch, record[epoch]
epoch += 1
folder = os.getcwd()
record_name = 'hrvo_multi.pkl'
fileObject = open(folder+"/"+record_name, 'wb')
pkl.dump(record, fileObject)
fileObject.close()
exit()
def start_hrvo(self):
env = gym_env.SubtCaveNobackEnv()
epoch = 0
iteration = 60
record = np.zeros([iteration, 2])
for i in range(iteration):
start_time = time.time()
ep_reward = 0
step = 0
distance = 0
while True:
v_des = compute_V_des(self.position, self.goal, self.v_max)
self.velocity = RVO_update(self.position, v_des, self.velocity,
self.ws_model)
dis, angle = self.process_ang_dis(self.velocity[0][0],
self.velocity[0][1],
self.yaw)
self.position, self.yaw, out_bound, r, done, info = env.step(
[dis * 0.4, angle * 0.4])
if out_bound or done:
self.goal = self.random_goal()
if done or env.total_dis > 600:
record[epoch][0] = env.total_dis
record[epoch][1] = time.time() - start_time
s = env.reset()
break
print epoch, record[epoch]
epoch += 1
folder = os.getcwd()
record_name = 'hrvo.pkl'
fileObject = open(folder + "/" + record_name, 'wb')
pkl.dump(record, fileObject)
fileObject.close()
V_max = [1.0 for i in xrange(len(X))]
# goal of [x,y]
goal = [[5.5-1.0*i, 5.0] for i in range(7)] + [[5.5-1.0*i, 0.0] for i in range(7)]
#------------------------------
#simulation setup
# total simulation time (s)
total_time = 15
# simulation step
step = 0.01
#------------------------------
#simulation starts
t = 0
while t*step < total_time:
# compute desired vel to goal
V_des = compute_V_des(X, goal, V_max)
# compute the optimal vel to avoid collision
V = RVO_update(X, V_des, V, ws_model)
# update position
for i in xrange(len(X)):
X[i][0] += V[i][0]*step
X[i][1] += V[i][1]*step
#----------------------------------------
# visualization
if t%10 == 0:
visualize_traj_dynamic(ws_model, X, V, goal, time=t*step, name='data/snap%s.png'%str(t/10))
#visualize_traj_dynamic(ws_model, X, V, goal, time=t*step, name='data/snap%s.png'%str(t/10))
t += 1
# goal of [x,y]
goal = [[0, 0], [0, 5], [5, 5], [5, 0]]
#------------------------------
#simulation setup
# total simulation time (s)
total_time = 15
# simulation step
step = 0.01
#------------------------------
#simulation starts
t = 0
while t * step < total_time:
# compute desired vel to goal
V_des = compute_V_des(X, goal, V_max)
# compute the optimal vel to avoid collision
V = RVO_update(X, V_des, V, ws_model)
# update position
for i in xrange(len(X)):
X[i][0] += V[i][0] * step
X[i][1] += V[i][1] * step
#----------------------------------------
# visualization
if t % 10 == 0:
#visualize_traj_dynamic(ws_model, X, V, goal, time=t*step, name='data/snap%s.pdf'%str(t/10))
visualize_traj_dynamic(ws_model,
X,
V,
goal,
time=t * step,
step = 0.01
# min timesteps to consider as having intent to interact
tau = 0.5
#------------------------------
#simulation starts
t = 0
# Compute desired velocity to goal
#V_des = compute_V_des(X, goal, V_max)
while t*step < total_time:
interacting = set()
for pi, pk in interacting_agents.items():
if pk:
interacting.add(pi)
if t == 0:
V_des = compute_V_des(X, goal, V_max, [], [])
print(V_des)
else:
V_des = compute_V_des(X, goal, V_max, interacting, V)
#print('V_des: ', V_des)
# compute the optimal vel to avoid collision
V, agents_within_soc_regs, interacting_agents = update_agents(X, V_des, V, ws_model,
agents_within_soc_regs,
interacting_agents, step, tau,
is_interact_agent)
#print(interacting_agents)
#print(V)
# update position
for i in range(len(X)):
X[i][0] += V[i][0]*step
