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