def get_path(self, loaded_paths):
# checkpoints are given as a list of shape [checkpoints,2]
full_path = PersonArray()
increments = self.velocity / self.publish_frequency
poses = loaded_paths.persons
for i in range(len(poses) - 1):
ex = np.array([poses[i].pose.position.x, poses[i].pose.position.y])
goua = np.array(
[poses[i + 1].pose.position.x, poses[i + 1].pose.position.y])
vector = goua - ex
orientation = np.arctan2(vector[1], vector[0])
length = np.linalg.norm(vector)
num_of_points = length / increments
x = np.linspace(0, vector[0],
num_of_points) + poses[i].pose.position.x
y = np.linspace(0, vector[1],
num_of_points) + poses[i].pose.position.y
for j, k in zip(x, y):
p = Person()
p.pose.position.x = j
p.pose.position.y = k
quatern = tf.transformations.quaternion_from_euler(
0, 0, orientation)
p.pose.orientation.z = quatern[-2]
p.pose.orientation.w = quatern[-1]
p.vel = poses[0].vel
p.id = poses[0].id
full_path.persons.append(p)
return full_path
def people_loop(self):
counters = [0] * self.no_of_people
reverse = [False] * self.no_of_people
while True:
array = PersonArray()
for i in range(self.no_of_people):
if reverse[i] == True:
counters[i] -= 1
elif reverse[i] == False:
counters[i] += 1
if counters[i] == len(self.dynamic_paths[i].persons):
reverse[i] = True
counters[i] -= 1
elif counters[i] == 0:
reverse[i] = False
if reverse[i] == True:
temp = copy.deepcopy(
self.dynamic_paths[i].persons[counters[i]])
z = temp.pose.orientation.z
w = temp.pose.orientation.w
temp.pose.orientation.z = -w
temp.pose.orientation.w = -z
array.persons.append(temp)
else:
array.persons.append(
self.dynamic_paths[i].persons[counters[i]])
self.person_publisher.publish(array)
self.publish_markers(array)
#self.publish_path_point(paths[0])
rospy.sleep(self.sleeptime)
def __init__(self):
self.listener = tf.TransformListener()
got_transform = False
while got_transform is False:
try:
self.trans, self.rot = self.listener.lookupTransform(
"world", "/map", rospy.Time.now())
got_transform = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
self.people_pub = rospy.Publisher("person_poses",
PersonArray,
queue_size=1)
self.sub1 = rospy.Subscriber("/vrpn_client_node/Person1/pose",
PoseStamped, self.cb1)
self.sub2 = rospy.Subscriber("/vrpn_client_node/Person2/pose",
PoseStamped, self.cb2)
self.sub3 = rospy.Subscriber("/vrpn_client_node/Person3/pose",
PoseStamped, self.cb3)
self.p_array = PersonArray()
self.people_local = [None, None, None]
self.ppl_pub = rospy.Timer(rospy.Duration(0.1), self.publish_people_cb)
self.timeout1 = rospy.Timer(rospy.Duration(0.4), self.cb_to1)
self.timeout2 = rospy.Timer(rospy.Duration(0.4), self.cb_to2)
self.timeout3 = rospy.Timer(rospy.Duration(0.4), self.cb_to3)
def publish_people_cb(self, event):
p_array = PersonArray()
for n, i in enumerate(self.people_local):
if i != None:
p_array.persons.append(i)
p_array.header.stamp = rospy.get_rostime()
p_array.header.frame_id = 'map'
self.people_pub.publish(p_array)
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 experiment_load_sevilla(directory,msg_avg = 50):
experiment = []
for subdir,dirs, files in os.walk(directory+'/traj_data/'):
for n,file in enumerate(files):
print "FILE",file
bag = rosbag.Bag(directory+'/traj_data/'+file)
ex = example()
person1=[];person2=[];goal = []
for topic, msg, t in bag.read_messages():
if topic == "/Robot_1/poseStamped":
msg.pose = zero_orientations_except_z(msg.pose)
ex.path.poses.append(msg)
ex.path_array.append(np.array([msg.pose.position.x,msg.pose.position.y]))
elif topic== "/Person_1/poseStamped":
msg.pose = zero_orientations_except_z(msg.pose)
person1.append(msg)
elif topic== "/Person_2/poseStamped":
msg.pose = zero_orientations_except_z(msg.pose)
person2.append(msg)
elif topic == "/Person_3/poseStamped":
msg.pose = zero_orientations_except_z(msg.pose)
goal.append(msg)
p1 = Person();p1.pose = person1[msg_avg].pose;p1.header.frame_id = "map";p1.id=1
p2 = Person();p2.pose = person2[msg_avg].pose;p2.header.frame_id = "map";p2.id=2
p = PersonArray();p.persons = [p1,p2];p.header.frame_id = "map"
ex.people = p
ex.goal = ex.path.poses[-1]#;#ex.goal = goal[int(len(goal)/2)]
ex.goal.header.frame_id = "map"
ex.goal_xy = np.array([ex.goal.pose.position.x,ex.goal.pose.position.y])
bag.close()
ex.nbrs.fit(ex.path_array)
experiment.append(deepcopy(ex))
good_traj = [0,1,2,4,6,7,9,10,11,16,17,20,22,23,24,25,26,27,28,30,31,34,36,37,38,39,43]
e = []
for i in good_traj:
e.append(experiment[i])
e.pop(7)
return e
def publish_random_position(self):
p_array = PersonArray()
for i in range(len(self.ppl_paths)):
p = Person()
p.header.frame_id = "map"
p.header.stamp = rospy.get_rostime()
rand = np.random.randint(low=0,
high=len(self.ppl_paths[i].persons),
size=1)
p.pose = self.ppl_paths[i].persons[rand].pose
p.vel = 0 # random points are always for 0 velocity.
p_array.persons.append(p)
p_array.header.frame_id = "map"
p_array.header.stamp = rospy.get_rostime()
for i in range(2):
self.person_publisher.publish(p_array)
self.publish_markers(p_array)
rospy.sleep(0.1)
def __init__(self, name):
# specify number of goals to collect
self.checkpoints_per_person = rospy.get_param(
"~checkpoints_per_person", 3)
# specify number of people
self.no_of_people = rospy.get_param("~number_of_people", 3)
self.save_name = name
# create data containers
self.point_pub = rospy.Publisher('all_clicked',
MarkerArray,
queue_size=1)
self.all_points = MarkerArray()
self.people = []
for i in range(self.no_of_people):
p = PersonArray()
self.people.append(p)
# specify name of file to be save
self.current_person = 0
# to stop spinning
self.done = False
def initialise_person_markers(self):
self.person_markers = MarkerArray()
p_array = PersonArray()
for i in range(self.no_of_people):
marker = Marker()
p = Person()
p.id = i
p.header.stamp = rospy.get_rostime()
marker.type = Marker.ARROW
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = "map"
marker.id = i
marker.pose = self.ppl_paths[i].persons[0].pose
marker.pose.position.z = 0.7
marker.scale.x = 0.6
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.b = 1.0
p.pose = self.ppl_paths[i].persons[0].pose
self.person_markers.markers.append(marker)
p_array.persons.append(p)
self.person_publisher.publish(p_array)
self.marker_publisher.publish(self.person_markers)
def handle_ppl_position_change(self, ppl_array):
person_markers = MarkerArray()
p = PersonArray()
p = ppl_array.ppl_array
p.header.stamp = rospy.get_rostime()
for n, i in enumerate(p.persons):
i.header.stamp = rospy.get_rostime()
i.id = int(n)
marker = Marker()
marker.type = Marker.ARROW
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = "map"
marker.id = n
marker.pose = i.pose
marker.pose.position.z = 0.7
marker.scale.x = 0.6
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.b = 1.0
person_markers.markers.append(marker)
self.ppl_pub.publish(p)
self.marker_pub.publish(person_markers)
rospy.sleep(0.1)