Пример #1
0
 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
Пример #2
0
 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
Пример #3
0
    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)
Пример #4
0
    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)