def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
return self.pos.header.frame_id, p
def get_person(self):
person = Person()
person.name = self.name
with self.lock:
person.position.x = self.position[0]
person.position.y = self.position[1]
return person
def sub_cb(self, msg):
self.fb_people.header.stamp = rospy.Time.now()
for box in msg.bounding_boxes:
if box.Class != "truck": # and box.probability > 0.2:
if box.Class in self.found_objects:
self.fb_people.people[self.found_objects[
box.Class]].position.x = random.random() * 10
self.fb_people.people[self.found_objects[
box.Class]].position.y = random.random() * 10
else:
self.found_objects[box.Class] = len(self.fb_people.people)
fb_person = Person()
print box.Class, " Found"
p1 = PoseStamped()
p1.header.frame_id = "base_link"
p1.pose.position.x = random.random() * 10
p1.pose.position.y = random.random() * 10
p1.pose.orientation.w = 1.0 # Neutral orientation
#p_in_base = self.tf_listener.transformPose("map", p1)
#fb_person.position.x = p_in_base.pose.position.x
#fb_person.position.y = p_in_base.pose.position.y
fb_person.position.x = p1.pose.position.x
fb_person.position.y = p1.pose.position.y
fb_person.name = str(box.Class)
self.fb_people.people.append(fb_person)
self.pub.publish(self.fb_people)
def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
return self.pos.header.frame_id, p
def publish_human_position(self): #人の座標を出力
obstacle_people = People() #people_msg
obstacle_people.header.frame_id = 'map'
obstacle_people.header.stamp = self.rospy_.Time.now()
marker_array_vis = MarkerArray() #people_visalizaion
ids = map(lambda x: x.id, self.human_class)
vanished_ids = list(set(self.prev_ids) - set(ids))
self.prev_ids = map(lambda x: x.id, self.human_class)
for i in vanished_ids:
# xrange(10000):
marker_vis = Marker() #marker
marker_vis.header.frame_id = 'map'
marker_vis.header.stamp = self.rospy_.Time.now()
marker_vis.id = i
marker_vis.type = 3 #3 cylinder 9 text
marker_vis.action = 2 #delete
marker_array_vis.markers.append(marker_vis)
self.human_vis_pub_.publish(marker_array_vis)
marker_array_vis = MarkerArray() #people_visalizaion
for i in range(0, len(self.human_class)):
tmp_human = Person() #person
tmp_human.name = str(self.human_class[i].id)
tmp_human.position.x = self.human_class[i].x
tmp_human.position.y = self.human_class[i].y
tmp_human.position.z = 0.0
tmp_human.velocity.x = self.human_class[i].x_vel
tmp_human.velocity.y = self.human_class[i].y_vel
tmp_human.velocity.z = 0.0
tmp_human.reliability = 1.0
obstacle_people.people.append(tmp_human)
marker_vis = Marker() #marker
marker_vis.header.frame_id = 'map'
marker_vis.header.stamp = self.rospy_.Time.now()
marker_vis.id = self.human_class[i].id
marker_vis.type = 3 #3 cylinder 9 text
marker_vis.action = 0 #add
marker_vis.scale.x = 0.1
marker_vis.scale.y = 0.1
marker_vis.scale.z = 1.0
marker_vis.color.r = 1
marker_vis.color.g = 0
marker_vis.color.b = 0
marker_vis.color.a = 1.0
marker_vis.lifetime = self.rospy_.Duration(0.2)
marker_vis.pose.position.x = self.human_class[i].x
marker_vis.pose.position.y = self.human_class[i].y
marker_vis.pose.position.z = 0.5
marker_vis.pose.orientation.x = 0.0
marker_vis.pose.orientation.y = 0.0
marker_vis.pose.orientation.z = 0.0
marker_vis.pose.orientation.w = 1.0
#marker_vis.text="aaaaaaaaa"
marker_array_vis.markers.append(marker_vis)
self.human_pub_.publish(obstacle_people)
self.human_vis_pub_.publish(marker_array_vis)
def publish_pose(self):
self.fb_people.header.stamp = rospy.Time.now()
fb_person = Person()
p1 = PoseStamped()
p1.header.frame_id = "base_link"
p1.pose.position.x = 1
p1.pose.orientation.w = 1.0 # Neutral orientation
p_in_base = self.tf_listener.transformPose("map", p1)
fb_person.position.x = p_in_base.pose.position.x
fb_person.position.y = p_in_base.pose.position.y
fb_person.name = str(fb_person.position.x + fb_person.position.y)
rospy.loginfo("Object Found")
self.fb_people.people.append(fb_person)
self.pub.publish(self.fb_people)
def spin(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pv = Person()
pl = People()
pl.header.stamp = rospy.Time.now()
pl.header.frame_id = '/map'
pv.position.x = self.x
pv.position.y = self.y
pv.position.z = .5
pv.velocity.x = 0
pv.velocity.y = 0
pv.name = 'Steve'
pv.reliability = .90
pl.people.append(pv)
self.ppub.publish(pl)
rate.sleep()
def spin(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pv = Person()
pl = People()
pl.header.stamp = rospy.Time.now()
pl.header.frame_id = '/base_link'
pv.position.x = float(sys.argv[1])
pv.position.y = float(sys.argv[2])
pv.position.z = .5
pv.velocity.x = float(sys.argv[3])
pv.velocity.y = float(sys.argv[4])
pv.name = 'asdf'
pv.reliability = .90
pl.people.append(pv)
self.ppub.publish(pl)
rate.sleep()
def spin(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pv = Person()
pl = People()
pl.header.stamp = rospy.Time.now()
pl.header.frame_id = '/base_link'
pv.position.x = float(sys.argv[1])
pv.position.y = float(sys.argv[2])
pv.position.z = .5
pv.velocity.x = float(sys.argv[3])
pv.velocity.y = float(sys.argv[4])
pv.name = 'asdf'
pv.reliability = .90
pl.people.append(pv)
self.ppub.publish(pl)
rate.sleep()
def callback(self, data):
self.people.header = data.header
self.people.header.frame_id = "camera_rgb_optical_frame"
self.angles.header = data.header
person_list = data.bounding_boxes
temp = []
temp_angles = []
for ppl in person_list:
person = Person()
pose = Pose()
y = (ppl.xmax + ppl.xmin) / 2
# # Position relative to world (center of camera is 90 degrees)
# position_y = -0.090625*(y - 640) + 60
# Position relative to robot (center of camera is 0 degrees)
position_y = (y - 320) * 29 / 320
position_y_rad = position_y * math.pi / 180
# pose.position.x = 1
pose.position.y = position_y_rad
pose.orientation.w = 1
person.position.y = position_y
person.reliability = ppl.probability
person.name = ppl.Class + "_" + names.get_first_name()
temp_angles.append(pose)
temp.append(person)
self.people.people = temp
self.angles.poses = temp_angles
self.pub.publish(self.people)
self.pub2.publish(self.angles)
def predict(self, tracked_persons):
tracks = tracked_persons.tracks
track_dict = {}
for track in tracks:
#print track
#print track.pose.pose.position.x
track_dict[track.track_id] = [
track.pose.pose.position.x, track.pose.pose.position.y
]
del self.prev_frames[0]
self.prev_frames.append(track_dict)
if len(tracks) == 0:
return
input_data = self.__generate_input(tracks)
#print input_data.shape
#print input_data
grid_batch = getSequenceGridMask(input_data, self.dimensions,
self.saved_args.neighborhood_size,
self.saved_args.grid_size)
obs_traj = input_data[:self.obs_length]
obs_grid = grid_batch[:self.obs_length]
print "********************** PREDICT NEW TRAJECTORY ******************************"
complete_traj = self.social_lstm_model.sample(self.sess, obs_traj,
obs_grid,
self.dimensions,
input_data,
self.pred_length)
#print complete_traj
# Initialize the markers array
prediction_markers = MarkerArray()
# Publish them
people_predictions = PeoplePrediction()
for frame_index in range(self.pred_length):
people = People()
people.header.stamp = tracked_persons.header.stamp + rospy.Duration(
frame_index * self.time_resolution)
people.header.frame_id = tracked_persons.header.frame_id
predicted_frame_index = frame_index + self.obs_length
for person_index in range(self.max_pedestrians):
track_id = complete_traj[predicted_frame_index, person_index,
0]
x_coord = complete_traj[predicted_frame_index, person_index, 1]
y_coord = complete_traj[predicted_frame_index, person_index, 2]
if track_id == 0:
break
person = Person()
person.name = str(track_id)
point = Point()
point.x = x_coord
point.y = y_coord
person.position = point
people.people.append(person)
self.prediction_marker.header.frame_id = tracked_persons.header.frame_id
self.prediction_marker.header.stamp = tracked_persons.header.stamp
self.prediction_marker.id = int(track_id)
self.prediction_marker.pose.position.x = person.position.x
self.prediction_marker.pose.position.y = person.position.y
#self.prediction_marker.color.a = 1 - (frame_index * 1.0 / (self.pred_length * 1.0))
self.prediction_marker.color.a = 1.0
prediction_markers.markers.append(self.prediction_marker)
people_predictions.predicted_people.append(people)
#print people_predictions
self.pedestrian_prediction_pub.publish(people_predictions)
self.prediction_marker_pub.publish(prediction_markers)
def modelCallback(states_msg):
# People publisher
ppl_pub = rospy.Publisher('people', People, queue_size=1)
# Marker publisher
marker_pub = rospy.Publisher('people_viz_arr', MarkerArray, queue_size=1)
# List to hold the indexes of the people objects in the message
ppl_idxs = []
# Loop through the names array to determine how many people objects are in the sim and their indexes
for i in range(len(states_msg.name)):
model_name = states_msg.name[i]
# Check if the model name is that of person
if model_name[:-2] == "person_standing":
# Append the index to the list
ppl_idxs.append(i)
# # Marker array to hold the markers for each person
# marker_list = MarkerArray()
# # Create marker message and initialize with values for external person model
# people_marker = Marker()
# people_marker.header.frame_id = "map"
# # people_marker.ns = "model"
# people_marker.type = Marker.MESH_RESOURCE
# people_marker.action = Marker.MODIFY
# people_marker.mesh_resource = "package://social_nav_simulation/gazebo/models/human/meshes/standing.dae"
# people_marker.mesh_use_embedded_materials = True
# people_marker.scale.x = 1.0
# people_marker.scale.y = 1.0
# people_marker.scale.z = 1.0
# Create people and person msgs
all_people = People()
one_person = Person()
# Loop through all the indexes of the people models found
for i in range(len(ppl_idxs)):
# Get the index of the person model
idx = ppl_idxs[i]
# Add the person's model name
one_person.name = states_msg.name[idx]
# Add the person's x and y position
one_person.position.x = states_msg.pose[idx].position.x
one_person.position.y = states_msg.pose[idx].position.y
# Set name of marker
# people_marker.ns = one_person.name
# # Create marker at the person's position
# people_marker.pose.position.x = one_person.position.x
# people_marker.pose.position.y = one_person.position.y
# people_marker.pose.orientation = states_msg.pose[idx].orientation
# marker_list.markers.append(copy.deepcopy(people_marker))
# marker_pub.publish(people_marker)
# Add the person's velocity
one_person.velocity.x = states_msg.twist[idx].linear.x
one_person.velocity.y = states_msg.twist[idx].linear.y
# ! Need to append a deep copy of the person object or or else each one will get overwritten
# by the last person object appeneded to the list
person_to_append = copy.deepcopy(one_person)
# Add the person to the people message
all_people.people.append(person_to_append)
# Add paraent frame and time to header
all_people.header.frame_id = "map"
all_people.header.stamp = rospy.Time.now()
# Publish list of all people
ppl_pub.publish(all_people)
def get_bbox_cog(self, yolo_boxes, cloud):
# Clear the people array
self.people.people = list()
# Clear the people pose array
self.people_poses.poses = list()
# Clear the bounding box array
self.people_bounding_boxes.boxes = list()
# Clear the detected persons array
self.detected_persons.detections = list()
# Assign arbitrary IDs
person_id = 0
for detection in yolo_boxes.bboxes:
if detection.Class == 'person':
bbox = [
detection.xmin, detection.ymin, detection.xmax,
detection.ymax
]
#try:
person_pose, person_bounding_box = self.get_bbox_pose(
bbox, cloud)
if person_pose is None:
continue
#except:
# print person_pose, person_bounding_box
# os._exit(1)
# Pose
self.people_poses.poses.append(person_pose.pose)
# Bounding box
self.people_bounding_boxes.boxes.append(person_bounding_box)
# Person message type for social cost map
person = Person()
person.position = person_pose.pose.position
#person.position.z = 0.0
person.name = str(len(self.people.people))
person.reliability = 1.0
# Spencer DetectedPerson
detected_person = DetectedPerson()
detected_person.modality = DetectedPerson.MODALITY_GENERIC_RGBD
detected_person.detection_id = person_id
person_id += 1
detected_person.pose.pose = person_pose.pose
self.detected_persons.detections.append(detected_person)
self.people.people.append(person)
now = rospy.Time.now()
self.people.header.stamp = now
self.people_pub.publish(self.people)
self.people_poses.header.stamp = now
self.people_poses_pub.publish(self.people_poses)
self.people_bounding_boxes.header.stamp = now
self.people_bounding_boxes_pub.publish(self.people_bounding_boxes)
self.detected_persons.header.stamp = now
self.detected_persons_pub.publish(self.detected_persons)
