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):
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 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_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)
