Ejemplo n.º 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
 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
Ejemplo n.º 3
0
    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
Ejemplo n.º 4
0
    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)
Ejemplo n.º 5
0
    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)
Ejemplo n.º 6
0
    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)
Ejemplo n.º 7
0
    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)
Ejemplo n.º 9
0
    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()
Ejemplo n.º 10
0
 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()
Ejemplo n.º 11
0
    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)
Ejemplo n.º 13
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
Ejemplo n.º 14
0
    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
Ejemplo n.º 17
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)
Ejemplo n.º 18
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)
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)