def reset(self, px, pz, rw):
### room
position = Vector3()
position.x = px #4.1
position.y = 0.1
position.z = pz #2.0 #2.8
#position = Vector3()
#position.x = 0.0
#position.y = 0.1
#position.z = 0.0
rotation = Quaternion()
rotation.x = 0
#rotation.y = random.uniform(-3.14, 3.14)
rotation.y = 1
rotation.z = 0
rotation.w = rw #PI*3/4+random.uniform(0, PI/2) #-3.14, 3.14) #-PI*4/5
done = motor.set_velocity(0, 0, 0, 0)
while done is False:
time_step.time_step_call()
done = motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
pub_set_position_req(position)
while robot_global.set_position_res_flag is False:
time_step.time_step_call()
pub_set_rotation_req(rotation)
while robot_global.set_rotation_res_flag is False:
time_step.time_step_call()
laser_data, done = laser.get_laser_scan_data()
while done is False:
laser_data, done = laser.get_laser_scan_data()
time_step.time_step_call()
self.action_history1 = 0
self.action_history2 = 0
self.action_history3 = 0
action_history = [
self.action_history1, self.action_history2, self.action_history3
]
#get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
return 0 #np.asarray(state)
def test(self, action, times):
if action == 0:
motor.set_velocity(5.0, 5.0, 5.0, 5.0)
elif action == 1:
motor.set_velocity(7.0, 7.0, 3.0, 3.0)
elif action == 2:
motor.set_velocity(3.0, 3.0, 7.0, 7.0)
elif action == 3:
motor.set_velocity(3.0, 3.0, -3.0, -3.0)
elif action == 4:
motor.set_velocity(-3.0, -3.0, 3.0, 3.0)
#laser_data, done = laser.get_laser_scan_data()
#while done is False:
# laser_data, done = laser.get_laser_scan_data()
for m in range(0, times):
time_step.time_step_call()
except:
pass
rospy.loginfo("robot %s connect success", model_name.data)
return model_name.data
rospy.init_node('webots_robot_server', anonymous=True)
robot_global.robot_name = robot_connect()
laser.init()
for i in range(0, 3):
time_step.time_step_call()
motor.init()
motor.set_velocity(0, 0, 0, 0)
for i in range(0, 3):
time_step.time_step_call()
step_cnt = 0
while True:
motor.set_velocity(3, 3, 3, 3)
time_step.time_step_call()
time_step.time_step_call()
time_step.time_step_call()
laser_data, done = laser.get_laser_scan_data()
while done is False:
robot_global.SubResetNodePhsicsRes = rospy.Subscriber(
'/simulation_reset_node_physics_res', Bool,
reset_node_physics_res_callback)
robot_global.PubGetPositionReq = rospy.Publisher(
'/simulation_get_position_req', Bool, queue_size=1)
robot_global.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_position_res', Vector3, get_position_res_callback)
robot_global.PubGetRotationReq = rospy.Publisher(
'/simulation_get_rotation_req', Bool, queue_size=1)
robot_global.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_rotation_res', Quaternion, get_rotation_res_callback)
motor.init()
motor.set_velocity(0, 0, 0, 0)
for i in range(0, 3):
time_step.time_step_call()
while True:
position = Vector3()
position.x = random.uniform(-2, 2)
position.y = 0.1
position.z = random.uniform(-2, 2)
rotation = Quaternion()
rotation.x = 0
rotation.y = random.uniform(-3.14, 3.14)
rotation.z = 0
rotation.w = 1
def reset(self,
testxml=0,
xml_human_x=-1,
xml_human_y=-1,
xml_human_rotation_z=-1,
xml_robot_x=-1,
xml_robot_y=-1,
xml_robot_rotation_z=-1):
if testxml == 0:
for random_iter in range(20):
if self.my_case == -1:
print("hjktest---- reset this", random_iter)
randomrobotINT = random.randint(0, 9)
if randomrobotINT == 0:
robotx = 1.5
roboty = 0.1
robotz = 1.5
elif randomrobotINT == 1:
robotx = 2
roboty = 0.1
robotz = 1.5
elif randomrobotINT == 2:
robotx = 3
roboty = 0.1
robotz = 1.5
else:
robotx = random.random() * (5.5 - 2) + 2
roboty = 0.1
robotz = random.random() * (8 - 4) + 4
#robotx = random.random() * (5 - 1.5) + 1.5
#roboty = 0.1
#robotz = random.random() * (8 - 1.5) + 1.5
'''
robotx = random.random() * (5.5 - 3.8) + 3.8
roboty = 0.1
robotz = random.random() * (4.5 - 2.7) + 2.7
'''
robot_rotat_x = 0
robot_rotat_y = 1
robot_rotat_z = 0
robot_rotat_w = random.uniform(-PI,
PI) #-3.14, 3.14) #-PI*4/5
randomINT = random.randint(0, 4)
if randomINT == 0:
humanx = 3
humany = 1.27
humanz = 6
elif randomINT == 1:
humanx = 4.5
humany = 1.27
humanz = 7.5
elif randomINT >= 2 and randomINT <= 4:
humanx = 2.5
humany = 1.27
humanz = 2
else:
humanx = random.random() * (4 - 2.5) + 2.5
humany = 1.27
humanz = random.random() * (7.5 - 2.5) + 2.5
else:
robotx = my_case_robotx[self.my_case]
roboty = my_case_roboty[self.my_case]
robotz = my_case_robotz[self.my_case]
robot_rotat_x = my_case_robot_rotat_x[self.my_case]
robot_rotat_y = my_case_robot_rotat_y[self.my_case]
robot_rotat_z = my_case_robot_rotat_z[self.my_case]
robot_rotat_w = my_case_robot_rotat_w[self.my_case]
humanx = my_case_humanx[self.my_case]
humany = my_case_humany[self.my_case]
humanz = my_case_humanz[self.my_case]
###important!!!!!
global global_rn
global_rn = random.random()
### room
position = Vector3()
position.x = robotx
position.y = roboty
position.z = robotz
rotation = Quaternion()
rotation.x = robot_rotat_x
#rotation.y = random.uniform(-3.14, 3.14)
rotation.y = robot_rotat_y
rotation.z = robot_rotat_z
rotation.w = robot_rotat_w
human_pose = Vector3()
human_pose.x = humanx
human_pose.y = humany
human_pose.z = humanz
rpos, rrot = getopposite(human_pose, human.rotation,
self.my_case)
dist1 = getDisXZ(position.x, position.z, rotation.w, rpos.x,
rpos.z, rrot.w)
distRtoH = getDisXZ(position.x, position.z, rotation.w,
human_pose.x, human_pose.z, 0)
if self.my_case != -1 or (dist1 < DISFROMSTARTTOEND
and distRtoH > MINDISFROMSTARTTOEND):
break
elif testxml == 1:
position = Vector3()
position.x = xml_robot_x
position.y = 0.1
position.z = xml_robot_y
rotation = Quaternion()
rotation.x = 0
# rotation.y = random.uniform(-3.14, 3.14)
rotation.y = 1
rotation.z = 0
rotation.w = xml_robot_rotation_z
human_pose = Vector3()
human_pose.x = xml_human_x
human_pose.y = 1.27
human_pose.z = xml_human_y
global global_rn
if xml_human_rotation_z == 1:
global_rn = 0
elif xml_human_rotation_z == 2:
global_rn = 0.6
elif xml_human_rotation_z == 3:
global_rn = 0.2
elif xml_human_rotation_z == 4:
global_rn = 0.9
done = motor.set_velocity(0, 0, 0, 0)
while done is False:
time_step.time_step_call()
done = motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
pub_set_position_req(position)
while robot_global.set_position_res_flag is False:
time_step.time_step_call()
pub_set_rotation_req(rotation)
while robot_global.set_rotation_res_flag is False:
time_step.time_step_call()
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
#print('hjk--- not set random :', human.position.x, human.position.y, human.position.z, human.rotation.x, human.rotation.y, human.rotation.z, human.rotation.w)
pub_set_human_pose_req(human_pose)
print("hjk--lll :", human.set_human_pose_res_flag)
while human.set_human_pose_res_flag is False:
time_step.time_step_call()
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
print('hjk--- have set random :', human.position.x, human.position.y,
human.position.z)
laser_data, done = laser.get_laser_scan_data()
while done is False:
laser_data, done = laser.get_laser_scan_data()
time_step.time_step_call()
laser_state, is_collision = discretize_observation(
laser_data, self.laser_dim, self.collision_threshold)
for i in range(0, 10):
time_step.time_step_call()
###############
self.action_history1 = 0
self.action_history2 = 0
self.action_history3 = 0
#get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
# self.mp = np.zeros((self.N,self.M))
rpos, rrot = getopposite(human.position, human.rotation, self.my_case)
dist1 = getDisXZ(robot_global.position.x, robot_global.position.z,
robot_global.rotation.w, rpos.x, rpos.z, rrot.w)
rtang, rot1, rot2 = getanglefea(position.x, position.z, rotation.w,
rpos.x, rpos.z, rrot.w)
print("11111 state dis : ", dist1 * 10)
state = changeStateFromEnvToNetwork(laser_state, dist1, rot1, rot2, -1)
self.distp = dist1
self.lenp = dist1
self.rot1p = rot1
self.rot2p = rot2
# self.old_robot = robot_global
self.old_robot_postion = copy.deepcopy(robot_global.position)
self.old_robot_rotation = copy.deepcopy(robot_global.rotation)
self.init_dist_pos = int(self.distp / 0.5)
#self.init_dist_pos = self.init_dist / 0.5
if self.init_dist_pos > self.wintimes_dist_key:
self.init_dist_pos = self.wintimes_dist_key
print("wwwww????? : ", self.init_dist_pos, self.distp)
self.wintimes_all[
self.init_dist_pos] = self.wintimes_all[self.init_dist_pos] + 1
return np.asarray(state)
def step(self, action):
if action == 0: # FORWARD
if self.action_history1 != 0:
motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
motor.set_velocity(5.0, 5.0, 5.0, 5.0)
elif action == 1: # LEFT forward
if self.action_history1 != 3:
motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
motor.set_velocity(4.0, 4.0, 7.0, 7.0)
elif action == 2: # RIGHT forward
if self.action_history1 != 4:
motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
motor.set_velocity(7.0, 7.0, 4.0, 4.0)
elif action == 3: # TURN LEFT
if self.action_history1 != 1:
motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
motor.set_velocity(-3.0, -3.0, 3.0, 3.0)
elif action == 4: # TURN RIGHT
if self.action_history1 != 2:
motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
motor.set_velocity(3.0, 3.0, -3.0, -3.0)
self.action_history3 = self.action_history2
self.action_history2 = self.action_history1
self.action_history1 = action
for i in range(0, 4):
time_step.time_step_call()
laser_data, done = laser.get_laser_scan_data()
while done is False:
laser_data, done = laser.get_laser_scan_data()
time_step.time_step_call()
laser_state, is_collision = discretize_observation(
laser_data, self.laser_dim, self.collision_threshold)
# get robot pose
pub_get_position_req()
while robot_global.get_position_res_flag is False:
time_step.time_step_call()
pub_get_rotation_req()
while robot_global.get_rotation_res_flag is False:
time_step.time_step_call()
# get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
# get right opposite pose to human
rpos, rrot = getopposite(human.position, human.rotation, self.my_case)
rtang, rot1, rot2 = getanglefea(robot_global.position.x,
robot_global.position.z,
robot_global.rotation.w, rpos.x,
rpos.z, rrot.w)
dist1 = getDisXZ(robot_global.position.x, robot_global.position.z,
robot_global.rotation.w, rpos.x, rpos.z, rrot.w)
# state = laser_state + [dist1, rot1, rot2] + action_history
#print ("from robot to person: ", robot_global.position.x, robot_global.position.z, robot_global.rotation.w,
# human.position.x, human.position.z, human.rotation.w, dist1, rot1)
# print ("State : " + str(state) + " Action : " + str(action))
state = changeStateFromEnvToNetwork(laser_state, dist1, rot1, rot2,
action)
'''
distance = getDisXZ(robot_global.position.x, robot_global.position.z, robot_global.rotation.w, self.old_robot.position.x, self.old_robot.position.z, self.old_robot.rotation.w)
#distance = getDisXZ(robot_global.position.x, robot_global.position.z, self.old_robot.position.x, self.old_robot.position.z, self.old_robot.rotation.w)
ACTION_LOG_FILE = open("my_net18/actionLog_" + strftime("%Y-%m-%d-%H-%M-%S", gmtime()) + ".txt", 'a+')
#print("???", file = ACTION_LOG_FILE)
print("postion change : ", distance, file=ACTION_LOG_FILE)
doubletmp = 1.0 * (robot_global.rotation.w - self.old_robot.rotation.w)
if doubletmp >= 2.0 * PI - 0.000001:
doubletmp = 0
if doubletmp <= -2.0 * PI + 0.000001:
doubletmp = 0
print("rotation value change : ", doubletmp, file=ACTION_LOG_FILE)
sys.stdout.flush()
self.old_robot_x = copy.deepcopy(robot_global)
'''
done = is_collision
if done is True:
reward = FAILREWARD
self.collisiontimes += 1
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print("----------------------------------")
print("----------------------------------")
print("-----NO! You collide it!------------")
print("----------------------------------")
print("----------------------------------")
print("----------------------------------")
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
print(
"------------------------------------------------------------------------------"
)
else:
reward = 0
tmp_par = dist1
if tmp_par > FACINGDIS_RANGE:
tmp_par = FACINGDIS_RANGE
print("hjk--- robot: ", robot_global.rotation.w, rrot.w)
print("hjk--- delta dis last - now : ", self.distp - dist1)
print("hjk--- delta angle last - now : ", math.fabs(self.rot1p),
math.fabs(rot1),
math.fabs(self.rot1p) - math.fabs(rot1))
print("hjk--- 2222222 delta angle last - now : ",
math.fabs(self.rot2p), math.fabs(rot2),
math.fabs(self.rot2p) - math.fabs(rot2))
if dist1 < 2.00:
reward = tmp_par * (
(self.distp - dist1) * RREWARDDIS +
(math.fabs(self.rot1p) - math.fabs(rot1)) * RREWARDANGLE
) + (FACINGDIS_RANGE - tmp_par) * RREWARDFACINGANGLE * (
math.fabs(self.rot2p) - math.fabs(rot2)
) # 0 + (self.difap - difa1)*10
print(
"hjk--reward: ",
tmp_par *
((self.distp - dist1) * RREWARDDIS +
(math.fabs(self.rot1p) - math.fabs(rot1)) * RREWARDANGLE),
(FACINGDIS_RANGE - tmp_par) * RREWARDFACINGANGLE *
(math.fabs(self.rot2p) - math.fabs(rot2)))
# print ("distance of past and this frame: " + str(self.distp) + " " + str(dist1))
# print ("angle difference of past and this frame: " + str(self.difap) + " " + str(difa1))
print("Rewards: " + str(reward))
self.lenp = dist1
else:
myspfa = spfa(self.mp, self.N, self.M, robot_global.position.x,
robot_global.position.z, rpos.x, rpos.z)
now_l, now_g_x, now_g_z = myspfa.getKey()
ang, _, _ = getanglefea(robot_global.position.x,
robot_global.position.z,
robot_global.rotation.w, now_g_x,
now_g_z, 0)
reward = (self.lenp - now_l) * RREWARDDIS + (
PI / 2.0 - math.fabs(ang)) * RREWARDANGLE
print("hjk--reward: ", (self.lenp - now_l) * RREWARDDIS,
(PI / 2.0 - math.fabs(ang)) * RREWARDANGLE, reward)
self.lenp = now_l
# time.sleep(10000000)
actionout = open(actionoutPath, 'a+')
print("action",
action,
"postionchange:",
getDisXZ(robot_global.position.x, robot_global.position.z, 0,
self.old_robot_postion.x, self.old_robot_postion.z,
0),
"rotationchange:",
np.fabs(robot_global.rotation.w - self.old_robot_rotation.w),
file=actionout)
sys.stdout.flush()
actionout.close()
self.old_robot_postion = copy.deepcopy(robot_global.position)
self.old_robot_rotation = copy.deepcopy(robot_global.rotation)
self.distp = dist1
self.rot1p = rot1
self.rot2p = rot2
if dist1 < ARRIVEDIS and math.fabs(rot2) < ARRIVEANGLE:
reward = WINREWARD
print(dist1)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print("**********************************")
print("**********************************")
print("*****OK! You shoot it!************")
print("**********************************")
print("**********************************")
print("**********************************")
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
print(
"******************************************************************************"
)
self.wintimes_win[self.init_dist_pos] = self.wintimes_win[
self.init_dist_pos] + 1
self.wintimes = self.wintimes + 1
done = True
#exit()
print("*********** hjk-- already shot win for all episode: ",
self.wintimes, "collision: ", self.collisiontimes,
self.wintimes_all, self.wintimes_win)
return np.asarray(state), reward, done, {}
def __init__(self, laser_dim, collision_threshold):
self.my_case = -1
global global_rn
global_rn = random.random()
self.laser_dim = laser_dim
self.collision_threshold = collision_threshold
rospy.init_node('webots_env', anonymous=True)
robot_global.robot_name = robot_connect()
human.human_name = human_connect()
robot_global.PubSetPositionReq = rospy.Publisher(
'/simulation_set_position_req', Vector3, queue_size=1)
robot_global.SubSetPositionRes = rospy.Subscriber(
'/simulation_set_position_res', Bool, set_position_res_callback)
###new add for human_pose
human.PubSetPositionReq = rospy.Publisher(
'/simulation_set_human_pose_req', Vector3, queue_size=1)
human.SubSetPositionRes = rospy.Subscriber(
'/simulation_set_human_pose_res', Bool,
set_human_pose_res_callback)
# human.PubSetRotationReq = rospy.Publisher('/simulation_set_rotation_req', Quaternion, queue_size=1)
#human.SubSetRotationRes = rospy.Subscriber('/simulation_set_rotation_res', Bool,
# set_rotation_res_callback)
robot_global.PubSetRotationReq = rospy.Publisher(
'/simulation_set_rotation_req', Quaternion, queue_size=1)
robot_global.SubSetRotationRes = rospy.Subscriber(
'/simulation_set_rotation_res', Bool, set_rotation_res_callback)
robot_global.PubResetNodePhsicsReq = rospy.Publisher(
'/simulation_reset_node_physics_req', Bool, queue_size=1)
robot_global.SubResetNodePhsicsRes = rospy.Subscriber(
'/simulation_reset_node_physics_res', Bool,
reset_node_physics_res_callback)
robot_global.PubGetPositionReq = rospy.Publisher(
'/simulation_get_position_req', Bool, queue_size=1)
robot_global.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_position_res', Vector3, get_position_res_callback)
robot_global.PubGetRotationReq = rospy.Publisher(
'/simulation_get_rotation_req', Bool, queue_size=1)
robot_global.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_rotation_res', Quaternion,
get_rotation_res_callback)
human.PubGetPositionReq = rospy.Publisher(
'/simulation_get_human_position_req', Bool, queue_size=1)
human.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_human_position_res', Vector3,
get_human_position_res_callback)
human.PubGetRotationReq = rospy.Publisher(
'/simulation_get_human_rotation_req', Bool, queue_size=1)
human.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_human_rotation_res', Quaternion,
get_human_rotation_res_callback)
for i in range(0, 5):
time_step.time_step_call() #zuoyong? hjk
motor.init()
motor.set_velocity(0, 0, 0, 0)
time_step.time_step_call()
time_step.time_step_call()
time_step.time_step_call()
laser.init()
#laser.get_laser_scan_data()
self.reward_range = (-np.inf, np.inf)
self.action_history1 = 0
self.action_history2 = 0
self.action_history3 = 0
for i in range(0, 5):
time_step.time_step_call()
#get robot pose
pub_get_position_req()
while robot_global.get_position_res_flag is False:
time_step.time_step_call()
pub_get_rotation_req()
while robot_global.get_rotation_res_flag is False:
time_step.time_step_call()
#get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
rpos, rrot = getopposite(human.position, human.rotation, self.my_case)
dist1 = getDisXZ(robot_global.position.x, robot_global.position.z,
robot_global.rotation.w, rpos.x, rpos.z, rrot.w)
rtang, rot1, rot2 = getanglefea(robot_global.position.x,
robot_global.position.z,
robot_global.rotation.w, rpos.x,
rpos.z, rrot.w)
self.distp = dist1 #past distance
self.rot1p = rot1
self.rot2p = rot2
self.init_dist_pos = 0
self.wintimes_dist_key = 10
self.wintimes_all = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.wintimes_win = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.wintimes = 0
self.collisiontimes = 0
self.N = int(6.6 * unit + 10)
self.M = int(9.9 * unit + 10)
self.mp = self.init_map_hjktest()
self.lenp = 0
self.old_robot_postion = copy.deepcopy(robot_global.position)
self.old_robot_rotation = copy.deepcopy(robot_global.rotation)
def __init__(self, laser_dim, collision_threshold):
self.laser_dim = laser_dim
self.collision_threshold = collision_threshold
rospy.init_node('webots_env', anonymous=True)
robot_global.robot_name = robot_connect()
human.human_name = human_connect()
robot_global.PubSetPositionReq = rospy.Publisher(
'/simulation_set_position_req', Vector3, queue_size=1)
robot_global.SubSetPositionRes = rospy.Subscriber(
'/simulation_set_position_res', Bool, set_position_res_callback)
robot_global.PubSetRotationReq = rospy.Publisher(
'/simulation_set_rotation_req', Quaternion, queue_size=1)
robot_global.SubSetRotationRes = rospy.Subscriber(
'/simulation_set_rotation_res', Bool, set_rotation_res_callback)
robot_global.PubResetNodePhsicsReq = rospy.Publisher(
'/simulation_reset_node_physics_req', Bool, queue_size=1)
robot_global.SubResetNodePhsicsRes = rospy.Subscriber(
'/simulation_reset_node_physics_res', Bool,
reset_node_physics_res_callback)
robot_global.PubGetPositionReq = rospy.Publisher(
'/simulation_get_position_req', Bool, queue_size=1)
robot_global.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_position_res', Vector3, get_position_res_callback)
robot_global.PubGetRotationReq = rospy.Publisher(
'/simulation_get_rotation_req', Bool, queue_size=1)
robot_global.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_rotation_res', Quaternion,
get_rotation_res_callback)
human.PubGetPositionReq = rospy.Publisher(
'/simulation_get_human_position_req', Bool, queue_size=1)
human.SubGetPositionRes = rospy.Subscriber(
'/simulation_get_human_position_res', Vector3,
get_human_position_res_callback)
human.PubGetRotationReq = rospy.Publisher(
'/simulation_get_human_rotation_req', Bool, queue_size=1)
human.SubGetRotationRes = rospy.Subscriber(
'/simulation_get_human_rotation_res', Quaternion,
get_human_rotation_res_callback)
for i in range(0, 5):
time_step.time_step_call()
motor.init()
motor.set_velocity(0, 0, 0, 0)
time_step.time_step_call()
time_step.time_step_call()
time_step.time_step_call()
laser.init()
#laser.get_laser_scan_data()
self.reward_range = (-np.inf, np.inf)
self.action_history1 = 0
self.action_history2 = 0
self.action_history3 = 0
self.goal_x = 0
self.goal_z = 0
self.dis_x = None
self.dis_z = None
self.dis_x_old = None
self.dis_z_old = None
#goal state
self.goalstate = 0
for i in range(0, 5):
time_step.time_step_call()
#get robot pose
pub_get_position_req()
while robot_global.get_position_res_flag is False:
time_step.time_step_call()
pub_get_rotation_req()
while robot_global.get_rotation_res_flag is False:
time_step.time_step_call()
#get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()