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 predict_path(self, person):
path_prediction = self.path_prediction = []
goal_x, goal_y = self.goal_pos
vx, vy = person.velocity.x, person.velocity.y
px, py = person.position.x, person.position.y
goal_dist = ((goal_x - px)**2 + (goal_y - py)**2)**0.5
speed = (vx**2 + vy**2)**0.5
theta = math.atan2(goal_y - py, goal_x - px)
step_dist = speed * self.time_resolution
if not speed:
max_prediction = 0
else:
max_prediction = min(self.num_predictions,
math.ceil(goal_dist / step_dist))
step_x, step_y = step_dist * math.cos(theta), step_dist * math.sin(
theta)
last_x, last_y = px, py
for i in range(1, self.num_predictions + 1):
person_prediction = Person()
person_prediction.position.z = 0
person_prediction.velocity = person.velocity
path_prediction.append(person_prediction)
if i >= max_prediction:
person_prediction.position.x = last_x
person_prediction.position.y = last_y
else:
last_x = person_prediction.position.x = px + i * step_x
last_y = person_prediction.position.y = py + i * step_y
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 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 viacon_callback(self, msg):
if self.last_pos is None:
self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec(
)
return
people = People()
person = Person()
people.header.stamp = msg.header.stamp
people.header.frame_id = "map"
last_x, last_y, last_t = self.last_pos
self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec(
)
if msg.header.stamp.to_sec() - last_t <= 0:
return
self.vx = (1 - self.gamma) * self.vx + \
self.gamma * (-msg.transform.translation.x - last_x) / (msg.header.stamp.to_sec() - last_t)
self.vy = (1 - self.gamma) * self.vy + \
self.gamma * (-msg.transform.translation.y - last_y) / (msg.header.stamp.to_sec() - last_t)
person.position.x = -msg.transform.translation.x
person.position.y = -msg.transform.translation.y
self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec(
)
people.people = [person]
self.p_pub.publish(people)
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 __init__(self, model_to_track_list):
# This are the models that we will generate information about.
self.gz_model_obj = GazeboModel(model_to_track_list)
self.people = People()
self.person = Person()
self.pub = rospy.Publisher("/people", People, queue_size=10)
self.pub2 = rospy.Publisher("/person_pose", People, queue_size=10)
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 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(self):
"""
"""
data = self.data
groups = []
group = []
persons = []
if not data.poses:
groups = []
else:
for pose in data.poses:
rospy.loginfo("Person Detected")
quartenion = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
(_, _,
yaw) = tf.transformations.euler_from_quaternion(quartenion)
pose_person = [pose.position.x, pose.position.y, yaw]
persons.append(pose_person)
if persons:
p = People()
p.header.frame_id = "/base_footprint"
p.header.stamp = rospy.Time.now()
for person in persons:
p1 = Person()
p1.position.x = person[0]
p1.position.y = person[1]
p1.velocity.x = 1 / math.tan(person[2])
p1.velocity.y = 1
p.people.append(p1)
self.pub.publish(p)
else:
p = People()
p.header.frame_id = "/base_footprint"
p.header.stamp = rospy.Time.now()
self.pub.publish(p)
def __init__(self):
self.r_ = rospy.Rate(100)
#initializing message
self.msg_tracked_persons_ = TrackedPersons()
self.msg_people_ = People()
self.msg_person_ = Person()
#Subscriber setup
rospy.Subscriber('/pedsim/tracked_persons', TrackedPersons,
self.callbackTrackedPersons)
#Publisher setup
self.pub_people_ = rospy.Publisher('/people', People, queue_size=10)
#Dummy flag
self.FIRST_MSG_ = False
class Recognizer(object):
def __init__(self, model, cascade_filename, run_local, wait, rp):
self.rp = rp
self.wait = wait
self.doRun = True
self.model = model
self.restart = False
self.ros_restart_request = False
self.detector = CascadedDetector(cascade_fn=cascade_filename,
minNeighbors=5,
scaleFactor=1.1)
if run_local:
print ">> Error: Run local selected in ROS based Recognizer"
sys.exit(1)
else:
self.bridge = CvBridge()
def signal_handler(signal, frame):
print ">> ROS Exiting"
self.doRun = False
signal.signal(signal.SIGINT, signal_handler)
def image_callback(self, ros_data):
try:
cv_image = self.bridge.imgmsg_to_cv2(ros_data, "bgr8")
except Exception, ex:
print ex
return
# Resize the frame to half the original size for speeding up the detection process.
# In ROS we can control the size, so we are sending a 320*240 image by default.
img = cv2.resize(cv_image, (320, 240), interpolation=cv2.INTER_CUBIC)
# img = cv2.resize(cv_image, (cv_image.shape[1] / 2, cv_image.shape[0] / 2), interpolation=cv2.INTER_CUBIC)
# img = cv_image
imgout = img.copy()
# Remember the Persons found in current image
persons = []
for _i, r in enumerate(self.detector.detect(img)):
x0, y0, x1, y1 = r
# (1) Get face, (2) Convert to grayscale & (3) resize to image_size:
face = img[y0:y1, x0:x1]
face = cv2.cvtColor(face, cv2.COLOR_BGR2GRAY)
face = cv2.resize(face,
self.model.image_size,
interpolation=cv2.INTER_CUBIC)
prediction = self.model.predict(face)
predicted_label = prediction[0]
classifier_output = prediction[1]
# Now let's get the distance from the assuming a 1-Nearest Neighbor.
# Since it's a 1-Nearest Neighbor only look take the zero-th element:
distance = classifier_output['distances'][0]
# Draw the face area in image:
cv2.rectangle(imgout, (x0, y0), (x1, y1), (0, 0, 255), 2)
# Draw the predicted name (folder name...):
draw_str(imgout, (x0 - 20, y0 - 40),
"Label " + self.model.subject_names[predicted_label])
draw_str(imgout, (x0 - 20, y0 - 20),
"Feature Distance " + "%1.1f" % distance)
msg = Person()
point = Point()
# Send the center of the person's bounding box
mid_x = float(x1 + (x1 - x0) * 0.5)
mid_y = float(y1 + (y1 - y0) * 0.5)
point.x = mid_x
point.y = mid_y
# Z is "mis-used" to represent the size of the bounding box
point.z = x1 - x0
msg.position = point
msg.name = str(self.model.subject_names[predicted_label])
msg.reliability = float(distance)
persons.append(msg)
if len(persons) > 0:
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = '/ros_cam'
msg = People()
msg.header = h
for p in persons:
msg.people.append(p)
self.rp.publisher.publish(msg)
cv2.imshow('OCVFACEREC < ROS STREAM', imgout)
cv2.waitKey(self.wait)
try:
z = self.ros_restart_request
if z:
self.restart = True
self.doRun = False
except Exception, e:
pass
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)
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)
