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