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