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"
