def __init__(self, ):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.lastDistance = 0 #QinjieLin
self.currentDistance = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
# self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
# self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
# self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.respawn_goal = Respawn()
self.resetX = rospy.get_param('/x_pos', 0.0)
self.resetY = rospy.get_param('/y_pos', 0.0)
self.resetZ = rospy.get_param('/z_pos', 0.0)
self.resetYaw = rospy.get_param('/yaw_angle', 0.0)
self.resetQua = quaternion_from_euler(0.0, 0.0, self.resetYaw)
self.startTime = time.time()
self.endTime = 0
self.numSuccess = 0
self.dis_weight = rospy.get_param('/dis_weight', 0.0)
self.lin_weight = rospy.get_param('/lin_weight', 0.0)
self.obs_weight = rospy.get_param('/obs_weight', 0.0)
self.ori_weight = rospy.get_param('/ori_weight', 0.0)
self.col_weight = rospy.get_param('/col_weight', 0.0)
self.goal_weight = rospy.get_param('/goal_weight', 0.0)
self.rot_weight = rospy.get_param('/rot_weight', 0.0)
self.laserStep = rospy.get_param('/laser_step', 60)
self.stepTime = rospy.get_param('/step_time', 0.5)
def __init__(self):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.lastDistance = 0 #QinjieLin
self.currentDistance = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.respawn_goal = Respawn()
self.resetX = rospy.get_param('/x_pos', 0.0)
self.resetY = rospy.get_param('/y_pos', 0.0)
self.resetZ = rospy.get_param('/z_pos', 0.0)
self.resetYaw = rospy.get_param('/yam_angle', 0.0)
self.resetQua = quaternion_from_euler(0.0, 0.0, self.resetYaw)
self.startTime = time.time()
self.endTime = 0
self.numSuccess = 0
self.thetaStep = rospy.get_param('/theta_step', 0.0)
self.thetaDistance = rospy.get_param('/theta_distance', 0.0)
self.thetaCollision = rospy.get_param('/theta_collision', 0.0)
self.thetaTurning = rospy.get_param('/theta_turning', 0.0)
self.thetaClearance = rospy.get_param('/theta_clearance', 0.0)
self.thetaGOal = rospy.get_param('/theta_goal', 0.0)
self.laserStep = rospy.get_param('/laser_step', 60)
def __init__(self, action_size):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.action_size = action_size
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.respawn_goal = Respawn()
def __init__(self, index=-1):
if (index == -1):
print("########## index == -1 ##########")
self.cmd_topic = "/cmd_vel"
self.odom_topic = "/odom"
self.scan_topic = "/base_scan"
self.crash_topic = "/is_crashed"
else:
self.cmd_topic = "robot_" + str(index) + "/cmd_vel"
self.odom_topic = "robot_" + str(index) + "/odom"
self.scan_topic = "robot_" + str(index) + "/base_scan"
self.crash_topic = "robot_" + str(index) + "/is_crashed"
self.reset_service = 'reset_positions'
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.lastDistance = 0
self.currentDistance = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher(self.cmd_topic, Twist, queue_size=5)
self.sub_odom = rospy.Subscriber(self.odom_topic, Odometry,
self.getOdometry)
self.sub_crash = rospy.Subscriber(self.crash_topic, Int8,
self.crash_callback)
self.reset_stage = rospy.ServiceProxy(self.reset_service, Empty)
self.respawn_goal = Respawn()
self.resetX = rospy.get_param('/x_pos', 0.0)
self.resetY = rospy.get_param('/y_pos', 0.0)
self.resetZ = rospy.get_param('/z_pos', 0.0)
self.resetYaw = rospy.get_param('/yaw_angle', 0.0)
self.resetQua = quaternion_from_euler(0.0, 0.0, self.resetYaw)
self.startTime = time.time()
self.endTime = 0
self.numSuccess = 0
self.dis_weight = rospy.get_param('/dis_weight', 0.0)
self.lin_weight = rospy.get_param('/lin_weight', 0.0)
self.obs_weight = rospy.get_param('/obs_weight', 0.0)
self.ori_weight = rospy.get_param('/ori_weight', 0.0)
self.col_weight = rospy.get_param('/col_weight', 0.0)
self.goal_weight = rospy.get_param('/goal_weight', 0.0)
self.rot_weight = rospy.get_param('/rot_weight', 0.0)
self.laserStep = rospy.get_param('/laser_step', 60)
self.stepTime = rospy.get_param('/step_time', 0.5)
self.realWorldFlag = rospy.get_param('/real_world')
self.simScale = 500.0
if (self.realWorldFlag):
self.simScale = 1.0
rospy.sleep(1.)
def __init__(self, action_size):
self.goal_x = 0
self.goal_y = 0
self.direction_of_movement = 0
self.scan_values = None
self.initGoal = True
self.max_scan_value = float(3.5)
self.action_size = action_size
self.get_goalbox = False
self.current_position = Pose()
self.pub_cmd_vel = rospy.Publisher('/mobile_base/commands/velocity',
Twist,
queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.respawn_goal = Respawn()
def __init__(self):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics',
Empty)
self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.respawn_goal = Respawn()
self.past_distance = 0.
#Keys CTRL + c will stop script
rospy.on_shutdown(self.shutdown)
def __init__(self):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.respawn_goal = Respawn()
self.past_distance = 0.
self.past_action = np.array([0.,0.])
obs_high = np.concatenate((np.array([3.5] * 10) ,np.array([0.22, 2.0]), np.array([10.]), np.array([10.]) ))
obs_low = np.concatenate((np.array([0.] * 10) ,np.array([0.0, -2.0]), np.array([-10.]), np.array([-10.]) ))
self.action_space = spaces.Box(low=np.array([0., -2.0]) ,high=np.array([0.22, 2.0]), dtype=np.float32)
self.observation_space = spaces.Box(low=obs_low , high=obs_high, dtype=np.float32)
#Keys CTRL + c will stop script
rospy.on_shutdown(self.shutdown)
def __init__(self):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher(
'/hydrone_aerial_underwater/cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber(
'/hydrone_aerial_underwater/ground_truth/odometry', Odometry,
self.getOdometry)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_world', Empty)
self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics',
Empty)
self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.respawn_goal = Respawn()
self.past_distance = 0.
self.stopped = 0
#Keys CTRL + c will stop script
rospy.on_shutdown(self.shutdown)
def __init__(self, action_size, prev_succ=0):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.action_size = action_size
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics',
Empty)
self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.set_pos_proxy = rospy.ServiceProxy('gazebo/set_model_state',
SetModelState)
self.respawn_goal = Respawn(prev_succ=prev_succ)
self.colliding = False
self.spawn_pos = ModelState()
self.spawn_pos.pose.position.x = BOT_INIT_X
self.spawn_pos.pose.position.y = BOT_INIT_Y
self.spawn_pos.model_name = "turtlebot3_burger"
step += 1
if __name__ == '__main__':
#comm = MPI.COMM_WORLD
#rank = comm.Get_rank()
#size = comm.Get_size()
rospy.init_node("turtlebot3_ppo_v3", anonymous=None)
filename = rospy.get_param('/filename', "autoRL_2")
load_ep = rospy.get_param('/load_ep', 800)
policy_path = os.path.dirname(os.path.abspath(
__file__)) + "/../../../../../../clever/saved_model_ppo/"
file = policy_path + filename + '/Stage1_%d' % load_ep
print("filename:", file)
respawn_goal = Respawn()
print("begin to load world")
env = TurtlebotWorld(OBS_SIZE, index=1, num_env=NUM_ENV)
print("generate the world")
reward = None
action_bound = [[0, -3], [1, 3]]
policy_path = 'policy'
# policy = MLPPolicy(obs_size, act_size)
policy = CNNPolicy(frames=LASER_HIST, action_space=2)
policy.cuda()
opt = Adam(policy.parameters(), lr=LEARNING_RATE)
mse = nn.MSELoss()
if not os.path.exists(policy_path):
