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()
#------------------------------
#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,
name='data/snap%s.png' % str(t / 10))
print("output result ", t / 10)
rospy.init_node('omni_controller')
sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0)
sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1)
sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2)
sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3)
pub = []
pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10))
pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10))
pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10))
pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10))
t = 0
while not rospy.is_shutdown():
V_des = compute_V_des(X, goal, V_max)
V = RVO_update(X, V_des, V, scenario)
for i in xrange(len(X)):
vel = Twist()
vel.linear.x = V[i][0]
vel.linear.y = V[i][1]
pub[i].publish(vel)
if t % 100:
print(X)
print('--------------')
t += 1
rospy.sleep(Ts)
total_time = 15
# simulation step
step = 0.05
#------------------------------
#simulation starts
t = 0
tool = Tools(4)
while t * step < total_time:
# compute desired vel to goal
goal = tool.get_goal(X, goals)
print(tool.visited)
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, local=2)
# update position
for i in range(len(X)):
X[i][0] += V[i][0] * step
X[i][1] += V[i][1] * step
#----------------------------------------
# visualization
if t % 2 == 0:
data = visualize_traj_dynamic(ws_model,
X,
V,
goal,
time=t * step,
name='data/snap%s.png' % str(t / 10),
save=True)
cv.imshow('image', data)