def __init__(self, name="record_skeletons", num_of_frames=1000): """Action Server to observe a location for a duration of time. Record humans being detected If > a number of frames, request consent to save images. """ rospy.loginfo("Skeleton Recording - starting an action server") self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, skeletonAction, \ execute_cb=self.execute_cb, auto_start=False) self._as.start() self.load_config() self.sk_publisher = SkeletonManagerConsent() # PTU state - based upon current_node callback self.ptu_action_client = actionlib.SimpleActionClient('/SetPTUState', PtuGotoAction) self.ptu_action_client.wait_for_server() # mongo store self.msg_store = MessageStoreProxy(database='message_store', collection='consent_images') self.number_of_frames_before_consent_needed = num_of_frames # gazing action server #self.gaze_client() self.publish_consent_pose = rospy.Publisher('skeleton_data/consent_pose', PoseStamped, queue_size = 10, latch=True) # topo nav move # self.nav_client() # speak self.speak() print ">>initialised recording action"
class skeleton_server(object): def __init__(self, name="record_skeletons", num_of_frames=1000): """Action Server to observe a location for a duration of time. Record humans being detected If > a number of frames, request consent to save images. """ rospy.loginfo("Skeleton Recording - starting an action server") self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, skeletonAction, \ execute_cb=self.execute_cb, auto_start=False) self._as.start() self.load_config() self.sk_publisher = SkeletonManagerConsent() # PTU state - based upon current_node callback self.ptu_action_client = actionlib.SimpleActionClient('/SetPTUState', PtuGotoAction) self.ptu_action_client.wait_for_server() # mongo store self.msg_store = MessageStoreProxy(database='message_store', collection='consent_images') self.number_of_frames_before_consent_needed = num_of_frames # gazing action server #self.gaze_client() self.publish_consent_pose = rospy.Publisher('skeleton_data/consent_pose', PoseStamped, queue_size = 10, latch=True) # topo nav move # self.nav_client() # speak self.speak() print ">>initialised recording action" def execute_cb(self, goal): print "send `start recording` page..." self.signal_start_of_recording() self.sk_publisher.reinisialise() self.sk_publisher.max_num_frames = self.number_of_frames_before_consent_needed duration = goal.duration start = rospy.Time.now() end = rospy.Time.now() consent_msg = "nothing" print "GOAL:", goal self.set_ptu_state(goal.waypoint) consented_uuid = "" request_consent = 0 while (end - start).secs < duration.secs and request_consent == 0: if self._as.is_preempt_requested(): break for cnt, (uuid, incr_msgs) in enumerate(self.sk_publisher.accumulate_data.items()): #print ">>", len(self.sk_publisher.accumulate_data[uuid]), uuid #publish the location of a person as a gaze request if cnt == 0 and len(incr_msgs) > 0: if incr_msgs[-1].joints[0].name == 'head': head = Header(frame_id='head_xtion_depth_optical_frame') look_at_pose = PoseStamped(header = head, pose=incr_msgs[-1].joints[0].pose) self.publish_consent_pose.publish(look_at_pose) # #self.gazeClient.send_goal(self.gazegoal) if len(incr_msgs) >= self.number_of_frames_before_consent_needed: request_consent = 1 consented_uuid = uuid self.sk_publisher.requested_consent_flag = 1 # stops the publisher storing data self.load_images_to_view_on_mongo(uuid) # uploads latest images to mongo print "breaking loop for: %s" % consented_uuid self.reset_ptu() self.speaker.send_goal(maryttsGoal(text=self.speech)) new_duration = duration.secs - (end - start).secs consent_msg = self.consent_client(new_duration) print "consent returned: %s: %s" % (consent_msg, consented_uuid) # break end = rospy.Time.now() # after the action reset ptu and stop publisher print "exited loop - %s" %consent_msg self.reset_ptu() # if no skeleton was recorded for the threshold if request_consent is 0: self.return_to_main_webpage() if self._as.is_preempt_requested(): print "The action is being preempted, cancelling everything. \n" self.return_to_main_webpage() return self._as.set_preempted() self._as.set_succeeded(skeletonActionResult()) if consent_msg is "everything": self.sk_publisher.consent_given_dump(consented_uuid) self.sk_publisher.reset_data() print "finished action\n" def consent_client(self, duration): rospy.loginfo("Creating consent client") ret = None start = rospy.Time.now() end = rospy.Time.now() try: consent_client = actionlib.SimpleActionClient('manage_consent', ManageConsentAction) if consent_client.wait_for_server(timeout=rospy.Duration(10)): goal = ManageConsentGoal() consent_client.send_goal(goal) # check whether you've been preempted, shutdown etc. while waiting for consent while not self._as.is_preempt_requested() and (end - start).secs < duration: if consent_client.wait_for_result(timeout = rospy.Duration(1)): result = consent_client.get_result() int_result = result.result.result if int_result == ConsentResult.DEPTH_AND_RGB: # print 'depth+rgb' ret = "everything" else: ret = "nothing" #elif int_result == ConsentResult.DEPTH_ONLY: # # print 'just depth' # ret = "depthskel" break end = rospy.Time.now() if (end - start).secs >= duration: print "timed out" if self._as.is_preempt_requested(): consent_client.cancel_all_goals() else: rospy.logwarn('No manage consent server') except Exception, e: rospy.logwarn('Exception when trying to manage consent: %s' % e) return ret
def __init__(self, name="record_skeletons", num_of_frames=1000): """Action Server to observe a location for a duration of time. Record humans being detected If > a number of frames, request consent to save images. """ rospy.loginfo("Skeleton Recording - starting an action server") self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, skeletonAction, \ execute_cb=self.execute_cb, auto_start=False) self._as.start() #self.load_config() self.number_of_frames_before_consent_needed = num_of_frames # skeleton publisher class (logs data given a detection) self.sk_publisher = SkeletonManagerConsent() # robot pose self.listen_to_robot_pose = 1 rospy.Subscriber("/robot_pose", Pose, callback=self.robot_callback, queue_size=10) # nav client self.nav_client = actionlib.SimpleActionClient('monitored_navigation', MonitoredNavigationAction) rospy.loginfo("Wait for monitored navigation server") self.nav_client.wait_for_server(rospy.Duration(60)) rospy.loginfo("Done") # PTU client self.ptu_height = 1.70 # assume camera is 1.7m self.ptu_client = actionlib.SimpleActionClient('SetPTUState', PtuGotoAction) rospy.loginfo("Wait for PTU action server") self.ptu_client.wait_for_server(rospy.Duration(60)) rospy.loginfo("Done") # mongo store self.msg_store = MessageStoreProxy(database='message_store', collection='consent_images') self.soma_id_store = MessageStoreProxy(database='message_store', collection='soma_activity_ids_list') self.soma_store = MessageStoreProxy(database="soma2data", collection="soma2") self.views_msg_store = MessageStoreProxy(collection='activity_view_stats') self.soma_roi_store = MessageStoreProxy(database='soma2data', collection='soma2_roi') # gazing action server self.gaze_client() self.publish_consent_pose = rospy.Publisher('skeleton_data/consent_pose', PoseStamped, queue_size = 10, latch=True) # Ferdi's stuff self._as_change = actionlib.SimpleActionClient( "/change_detector/action", ChangeDetectionAction ) self._as_change.wait_for_server() # Ferdi's stuff # speak self.speak() # auto select viewpoint for recording self.view_dist_thresh_low = 1.5 self.view_dist_thresh_high = 2.5 self.possible_nav_goals = [] # visualizing the view point goal in RVIZ self.pub_viewpose = rospy.Publisher('activity_view_goal', PoseStamped, queue_size=10) self.pub_all_views = rospy.Publisher('activity_possible_view_goals', PoseArray, queue_size=10) print ">>initialised recording action"