def laser_init():
global laser
if not laser.init():
print("Laser init failed.")
return False
print("Laser init passed.")
return True
def loop(g, s):
sprite.apply_gravity(g, s)
sprite.apply_standing(g, s)
#if s.rect.x == s._prev.x: # or sprite.get_code(g,s,sign(s.vx),0) == CODE_SHOOTBOT_TURN:
# s.vx = -s.vx
s._prev = pygame.Rect(s.rect)
if g.player.rect.centerx > s.rect.centerx:
s.vx += 0.01
elif g.player.rect.centerx < s.rect.centerx:
s.vx -= 0.01
if s.vx > 0.0:
s.facing = 'right'
elif s.vx < 0.0:
s.facing = 'left'
s.image = 'shootbot-%s-%s' % (s.facing, (g.frame / 10) % 4)
if sprite.get_code(g, s, sign(s.vx), 0) == CODE_SHOOTBOT_TURN:
s.vx = 0.0
s.vx = min(1.0, s.vx)
s.vx = max(-1.0, s.vx)
if s.shoot == 0:
shot = laser.init(g, s.rect, s)
g.sprites.append(shot)
s.shoot = 120
s.shooting = 10
if s.shooting > 0:
s.image = 'shootbot-%s-shoot' % (s.facing)
s.shooting -= 1
s.shoot -= 1
s.rect.x += sprite.myinc(g.frame, s.vx)
s.rect.y += sprite.myinc(g.frame, s.vy)
def loop(g,s):
sprite.apply_gravity(g,s)
sprite.apply_standing(g,s)
#if s.rect.x == s._prev.x: # or sprite.get_code(g,s,sign(s.vx),0) == CODE_GUARDIAN_TURN:
# s.vx = -s.vx
s._prev = pygame.Rect(s.rect)
if g.player.rect.centerx > s.rect.centerx:
s.vx += 0.02
elif g.player.rect.centerx < s.rect.centerx:
s.vx -= 0.02
if s.vx > 0.0:
s.facing = 'right'
elif s.vx < 0.0:
s.facing = 'left'
s.image = 'guardian/guardian-%s-%s' % (s.facing, (g.frame / 10) % 4)
if sprite.get_code(g,s,sign(s.vx),0) == CODE_GUARDIAN_TURN:
s.vx = 0.0
s.vx = min(1.0, s.vx)
s.vx = max(-1.0, s.vx)
if s.shoot == 0:
shot = laser.init(g,s.rect,s)
#g.sprites.append(shot)
s.shoot = 120
s.shooting = 10
if s.shooting > 0:
s.image = 'guardian/guardian-%s-shoot' % (s.facing)
s.shooting -= 1
s.shoot -= 1
s.rect.x += sprite.myinc(g.frame,s.vx)
s.rect.y += sprite.myinc(g.frame,s.vy)
while model_name is None:
try:
model_name = rospy.wait_for_message('/model_name',
String,
timeout=5)
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()
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)
configFile = open('Configurations/robot.cfg.default')
except:
log.error("No config file could be opened.")
configs.init(configFile)
config = configs.get_config()
log.debug("Using config file %s" % configFile.name)
# Initialize modules #
# Sabertooth2x10
import sabertooth2x10 as saber
saber.init()
mc = saber.get_object()
# Laser Range Finder
import laser
laser.init()
lrf = laser.get_object()
lrf.clear()
# MCN
import micro_controller_network as mcn
mcn.init()
micros = []
servo_micro = mcn.get_object('Servo Control')
micros.append(servo_micro)
od_micro = mcn.get_object('Obj Detection')
micros.append(od_micro)
array_micro = mcn.get_object('Array Micro')
micros.append(array_micro)
# Servo Controller
import servo_controller as sc
sc.init()
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()
