def __init__(self): self.exp_name = rospy.get_param("~experiment_name", "posq_test_exp3") self.session_name = rospy.get_param("~session_name", "posq_learn_small2") self.real_data = rospy.get_param("~real_data", True) self.no_of_runs = rospy.get_param("number_of_runs", 2) self.filepath = PARENT print PARENT, CURRENT self.directory = self.filepath + "/data/" + self.exp_name self.results_dir = self.filepath + "/results/" + self.session_name + "/" #self.real_data_eval() self.experiment_data = experiment_load2(self.directory) self.gt_weights = fn.pickle_loader(self.directory + "/weights.pkl") self.results = [] # print self.results_dir # print "gothere" for i in range(self.no_of_runs): self.results.append( fn.pickle_loader(self.results_dir + "results_" + str(i + 1) + ".pkl")) # self.results = self.results[:3] # self.no_of_runs=2 self.plots() # self.planner = MotionPlanner() # self.costlib = self.planner.cost_manager # self.ppl_pub = rospy.Publisher("person_poses",PersonArray,queue_size = 10) # self.expert_path_pub = rospy.Publisher("expert_path",Path,queue_size = 1) # self.initial_paths_pub = rospy.Publisher("initial_weights_path",Path,queue_size = 1) # self.final_paths_pub = rospy.Publisher("final_weights_path",Path,queue_size = 1) # self.point_sub = rospy.Subscriber("clicked_point",PointStamped,self.point_cb) # self.astar_03_path_pub = rospy.Publisher("astar03_path",Path,queue_size = 1) # self.astar_08_path_pub = rospy.Publisher("astar08_path",Path,queue_size = 1) # self.crlt_path_pub = rospy.Publisher("crlt_path",Path,queue_size = 1) self.visualization_counter = 0
def __init__(self): self.iterations = rospy.get_param("~iterations", 10) self.learning_rate = rospy.get_param("~learning_rate", 0.5) self.time_margin_factor = rospy.get_param("~time_margin_factor", 1.0) self.cache_size = rospy.get_param("~point_cache_size", 2500) self.momentum = 0.2 self.exp_name = rospy.get_param("~experiment_name", "sevilla_test") self.session_name = rospy.get_param("~session_name", "sevilla_learn1") self.baseline_eval = rospy.get_param("~baseline", False) self.path = PARENT self.directory = self.path+"/data/"+self.exp_name self.results_dir = self.path+"/results/"+self.session_name+"/" fn.make_dir(self.results_dir) self.experiment_data = experiment_load_sevilla(self.directory) self.planner = MotionPlanner() self.validation_proportion = rospy.get_param("~validation_proportion", 0.8) self.costlib = self.planner.cost_manager self.ppl_pub = rospy.Publisher("person_poses",PersonArray,queue_size = 10) if self.baseline_eval==True: try: loaded = fn.pickle_loader(self.directory+"/weights.pkl") self.gt_weights = loaded["weights"] self.gt_featureset = loaded["feature_set"] except: self.gt_weights=None self.baseline_eval=False self.write_learning_params(self.results_dir) self.sevilla_test_run("1")
def __init__(self): self.iterations = rospy.get_param("~iterations", 10) self.learning_rate = rospy.get_param("~learning_rate", 0.5) self.time_margin_factor = rospy.get_param("~time_margin_factor", 1.0) self.cache_size = rospy.get_param("~point_cache_size", 2500) self.momentum = 0.2 self.exp_name = rospy.get_param("~experiment_name", "sevilla_test") self.session_name = rospy.get_param("~session_name", "sevilla_learn1") self.baseline_eval = rospy.get_param("~baseline", False) self.path = PARENT self.directory = self.path + "/data/" + self.exp_name self.results_dir = self.path + "/results/" + self.session_name + "/" fn.make_dir(self.results_dir) self.experiment_data = experiment_load_sevilla(self.directory) self.planner = MotionPlanner() self.validation_proportion = rospy.get_param("~validation_proportion", 0.8) self.costlib = self.planner.cost_manager self.ppl_pub = rospy.Publisher("person_poses", PersonArray, queue_size=10) if self.baseline_eval == True: try: loaded = fn.pickle_loader(self.directory + "/weights.pkl") self.gt_weights = loaded["weights"] self.gt_featureset = loaded["feature_set"] except: self.gt_weights = None self.baseline_eval = False self.write_learning_params(self.results_dir) self.sevilla_test_run("1")
def __init__(self, mode, goal_directory, people_static=True): # load the goals from the correct file. self.goals = fn.pickle_loader(goal_directory + "/goals.pkl") self.current_goal = None self.max_goals = 20 self.goals_sent = 0 self.succeded = 0 self.goal_index = 100 self.mode = AUTO self.goal_directory = "test_experiment" fn.make_dir(goal_directory + '/traj_data/') rospy.on_shutdown(self.shutdown) #first bag file to record self.bag = rosbag.Bag(goal_directory + '/traj_data/test.bag', 'w') self.tf_listener = tf.TransformListener() self.people_static = people_static self.goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] # action client so send goals self.move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction) # Dealing with cases when people are not detected. self.new_people_message = False self.people_message_timeout = 0 self.people_latest = PersonArray() self.new_bag = False if self.mode == AUTO: #connect to movebase connected = False while not connected: try: self.move_base.wait_for_server(rospy.Duration(15)) connected = True except rospy.ServiceException: rospy.logwarn("could not connect. etc") #initialise extra markers and publisers for experiment self.initialise_marker() self.people_publisher = People_Publisher("test_experiment", static=True) self.pub1 = rospy.Publisher('visualise_goal', Marker, queue_size=200) self.pub2 = rospy.Publisher('visualise_orient', Marker, queue_size=200) # Robot position and people subscribers self.position_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.position_callback) self.pos_marker_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.pos_marker_callback) self.people_sub = rospy.Subscriber("/person_poses", PersonArray, self.people_callback) rospy.sleep(1.) self.random_goal_loop()
def __init__(self,mode,goal_directory,people_static = True): # load the goals from the correct file. self.goals = fn.pickle_loader(goal_directory+"/goals.pkl") self.current_goal = None self.max_goals = 20 self.goals_sent = 0 self.succeded = 0 self.goal_index = 100 self.mode = AUTO self.goal_directory = "test_experiment" fn.make_dir(goal_directory+'/traj_data/') rospy.on_shutdown(self.shutdown) #first bag file to record self.bag = rosbag.Bag(goal_directory+'/traj_data/test.bag','w') self.tf_listener = tf.TransformListener() self.people_static = people_static self.goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'] # action client so send goals self.move_base =actionlib.SimpleActionClient('move_base',MoveBaseAction) # Dealing with cases when people are not detected. self.new_people_message = False;self.people_message_timeout = 0; self.people_latest = PersonArray() self.new_bag = False if self.mode == AUTO: #connect to movebase connected = False while not connected: try: self.move_base.wait_for_server(rospy.Duration(15)) connected = True except rospy.ServiceException: rospy.logwarn("could not connect. etc") #initialise extra markers and publisers for experiment self.initialise_marker() self.people_publisher = People_Publisher("test_experiment",static = True) self.pub1 = rospy.Publisher('visualise_goal',Marker,queue_size = 200) self.pub2 = rospy.Publisher('visualise_orient',Marker,queue_size = 200) # Robot position and people subscribers self.position_sub = rospy.Subscriber("/amcl_pose",PoseWithCovarianceStamped,self.position_callback) self.pos_marker_sub = rospy.Subscriber("/amcl_pose",PoseWithCovarianceStamped,self.pos_marker_callback) self.people_sub = rospy.Subscriber("/person_poses",PersonArray,self.people_callback) rospy.sleep(1.) self.random_goal_loop()
def __init__(self,mode,goal_directory,people_static = True): # load the goals from the correct file. self.goals = fn.pickle_loader(goal_directory+"/goals.pkl") self.current_goal = None self.max_goals = rospy.get_param("~datapoints_to_collect", 5) self.goals_sent = 0 self.succeded = 0 self.goal_index = 100 self.goal_directory = goal_directory self.weights = None self.planner = MotionPlanner() weight_info = {"feature_set":self.planner.planning_featureset,"weights":self.planner.cost_manager.weights} fn.pickle_saver(weight_info,self.goal_directory+"/weights.pkl") fn.make_dir(goal_directory+'/traj_data/') rospy.on_shutdown(self.shutdown) #first bag file to record self.bag = rosbag.Bag(goal_directory+'/traj_data/test1.bag','w') self.tf_listener = tf.TransformListener() self.people_static = people_static #self.set_initial_position() # initial position set based on the first place in goals # action client so send goals self.goal_pub = rospy.Publisher('move_base_simple/goal',PoseStamped,queue_size=1) # Dealing with cases when people are not detected. self.new_people_message =False;self.people_message_timeout = 0; self.people_latest = PersonArray() self.new_bag = False #initialise extra markers and publisers for experiment self.initialise_marker() self.people_publisher = People_Publisher(self.goal_directory,static = True) self.pub1 = rospy.Publisher('visualise_goal',Marker,queue_size = 200) self.pub2 = rospy.Publisher('visualise_orient',Marker,queue_size = 200) # Robot position and people subscribers self.people_sub = rospy.Subscriber("/person_poses",PersonArray,self.people_callback) #self.weights_sub = rospy.Subscriber("weights",Float32MultiArray,self.weights_cb) rospy.sleep(4.) print "ABOUT TO ENTER THE LOOOOOOOPP__________________________________________________" self.random_goal_loop()
def __init__(self, people_paths_directory, static=True): self.ppl_paths = fn.pickle_loader(people_paths_directory + "/ppl_paths.pkl") self.static = static self.person_publisher = rospy.Publisher("person_poses", PersonArray, queue_size=10) self.marker_publisher = rospy.Publisher("person_markers", MarkerArray, queue_size=10) self.no_of_people = len(self.ppl_paths) rospy.sleep(0.5) self.initialise_person_markers() if static == False: self.dynamic_paths = [] self.publish_frequency = 5 # messages per second self.sleeptime = 1. / self.publish_frequency self.velocity = 0.1 for n, i in enumerate(self.ppl_paths): i.persons[0].vel = self.velocity i.persons[0].id = n p = self.get_path(i) self.dynamic_paths.append(p)
def real_data_eval(self): self.results = [] self.experiment_data = experiment_load_sevilla(self.directory) for i in range(self.no_of_runs): self.results.append( fn.pickle_loader(self.results_dir + "results_" + str(i + 1) + ".pkl")) self.planner = MotionPlanner() self.costlib = self.planner.cost_manager self.ppl_pub = rospy.Publisher("person_poses", PersonArray, queue_size=1) self.expert_path_pub = rospy.Publisher("expert_path", Path, queue_size=1) self.initial_paths_pub = rospy.Publisher("initial_weights_path", Path, queue_size=1) self.final_paths_pub = rospy.Publisher("final_weights_path", Path, queue_size=1) self.point_sub = rospy.Subscriber("clicked_point", PointStamped, self.point_cb_real)
def __init__(self, goal_directory): self.goals = fn.pickle_loader(goal_directory + "/goals.pkl") self.set_initial_position()