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):