def state_machine(self): current_state = self.WAITING_STATE while self.on_grid and not rospy.is_shutdown(): # public current state for web interface if current_state == self.WAITING_STATE: sleep(10) current_state = self.SUGGESTING_STATE elif current_state == self.SUGGESTING_STATE: box = [ i for i in sorted(self.on_grid, key=lambda x: x.difficulty) if i.difficulty > 0.00001 ] if box == []: self.talk.wait_for_server() self.talk.send_goal( maryttsGoal(text="Please move the final object")) self.talk.wait_for_result() else: box = box[0] print box.box_id, box.difficulty self.move_to(box.grid_position_x, box.grid_position_y) current_state = self.WAITING_STATE elif current_state == self.REPEATING_STATE: current_state = self.SUGGESTING_STATE print 'FINISHED'
def consent_ret_callback(self, msg): if self.request_sent_flag == 0: return print "got consent ret callback, %s" % msg self.consent_ret=msg # self.request_sent_flag = 0 # when the request is returned, go back to previous waypoint self.speaker.send_goal(maryttsGoal(text="Thank you")) self.go_back_to_where_I_came_from()
def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e)) self._error = True
def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text Logger.loginfo("Speech output, saying:\n%s" % goal.text) self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn("Failed to send the Sweep command:\n%s" % str(e)) self._error = True
def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text print "speech_output_state: say : ", goal.text self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn("Failed to send the Sweep command:\n%s" % str(e)) self._error = True
def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text print 'speech_output_state: say : ', goal.text self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e)) self._error = True
def move_to(self, x, y): box = [ i for i in self.object_list if i.grid_position_x == x and i.grid_position_y == y ][0] self.robot.move_to_joint_positions(self.natural_position) self.talk.wait_for_server() self.talk.send_goal( maryttsGoal(text=self.process_speech(box.category))) self.robot.move_to_joint_positions(self.grid_positions[x][y]) self.talk.wait_for_result() self.robot.move_to_joint_positions(self.natural_position)
def callback(self, msg): if msg.task.action[1:] in self.config.keys(): events = itertools.chain( self.all, self.config[msg.task.action[1:]]["events"]) else: events = self.all for event in events: execute = True if msg.event == eval("TaskEvent." + event.keys()[0]): if "compare" in event.values()[0].keys(): comps = event.values()[0]["compare"] for comp in comps: if not eval( str(comp["static_value"]) + comp["comparison"] + "msg." + comp["task_field"]): execute = False break if "compare_to_topic" in event.values()[0].keys(): comps = event.values()[0]["compare_to_topic"] for comp in comps: res = rospy.wait_for_message( comp["topic"], roslib.message.get_message_class( rostopic.get_topic_type(comp["topic"], True)[0])) if not eval("res." + comp["field"] + comp["comparison"] + "msg." + comp["task_field"]): execute = False break if execute: self.mary_client.send_goal_and_wait( maryttsGoal(text=event.values()[0]["text"])) rospy.loginfo("Saying: " + event.values()[0]["text"])
def say(self, what_to_say): self.talk.wait_for_server() self.talk.send_goal(maryttsGoal(text=str(what_to_say))) self.talk.wait_for_result()
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_ret_callback(self, msg): print "-> consent ret callback", self.request_sent_flag, msg if self.request_sent_flag != 1: return self.consent_ret=msg self.speaker.send_goal(maryttsGoal(text="Thank you"))
def execute_cb(self, goal): self.listen_to_robot_pose = 1 duration = goal.duration start = rospy.Time.now() end = rospy.Time.now() consent_msg = "nothing" rospy.loginfo("GOAL: %s", goal.waypoint) self.get_soma_roi() self.get_soma_objects() self.create_possible_navgoals() self.generate_viewpoints() nav_fail = self.goto() consented_uuid = "" request_consent = 0 # Ferdi's stuff # An action server to monitor changing during duration period # during this period, the process fluctuatively takes around 40-100% CPU process # (normally one CPU core) self._as_change.send_goal( ChangeDetectionGoal(goal.waypoint, False, goal.duration) ) # Ferdi's stuff rospy.loginfo("init recording page and skeleton pub") self.signal_start_of_recording() self.sk_publisher.reinisialise() self.sk_publisher.max_num_frames = self.number_of_frames_before_consent_needed while (end - start).secs < duration.secs and request_consent == 0: if self._as.is_preempt_requested(): self._as_change.cancel_all_goals() 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.upload_images_to_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 rospy.loginfo("exited loop. consent=%s" %consent_msg) # LOG THE STATS TO MONGO res = (request_consent, consent_msg) self.log_view_info(res, nav_fail, goal.waypoint, start, end) # reset everything: self.reset_everything() # 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() # Ferdi's stuff # just to make sure that Ferdi's action is closed properly self._as_change.wait_for_result(10) # Ferdi's stuff print "finished action\n"
def callback(self, msg, waypoint): self.inc_sk = msg if str(datetime.datetime.now().date()) != self.date: print 'new day!' self.date = str(datetime.datetime.now().date()) self.dir1 = '/home/' + getpass.getuser() + '/SkeletonDataset/pre_consent/'+self.date+'/' print 'checking if folder exists:',self.dir1 if not os.path.exists(self.dir1): print ' -create folder:',self.dir1 os.makedirs(self.dir1) # print self.inc_sk.uuid if self._flag_robot and self._flag_rgb and self._flag_rgb_sk: if self.inc_sk.uuid not in self.sk_mapping: self.sk_mapping[self.inc_sk.uuid] = {} self.sk_mapping[self.inc_sk.uuid]['frame'] = 1 self.sk_mapping[self.inc_sk.uuid]['time'] = str(datetime.datetime.now().time()).split('.')[0]+'_' t = self.sk_mapping[self.inc_sk.uuid]['time'] print ' -new skeleton detected with id:', self.inc_sk.uuid # print ' -creating folder:',t+self.inc_sk.uuid if not os.path.exists(self.dir1+t+self.inc_sk.uuid): os.makedirs(self.dir1+t+self.inc_sk.uuid) os.makedirs(self.dir1+t+self.inc_sk.uuid+'/rgb') os.makedirs(self.dir1+t+self.inc_sk.uuid+'/depth') os.makedirs(self.dir1+t+self.inc_sk.uuid+'/rgb_sk') os.makedirs(self.dir1+t+self.inc_sk.uuid+'/robot') os.makedirs(self.dir1+t+self.inc_sk.uuid+'/skeleton') # create the empty bag file (closed in /skeleton_action) self.bag_file = rosbag.Bag(self.dir1+t+self.inc_sk.uuid+'/detection.bag', 'w') t = self.sk_mapping[self.inc_sk.uuid]['time'] if os.path.exists(self.dir1+t+self.inc_sk.uuid): # setup saving dir and frame d = self.dir1+t+self.inc_sk.uuid+'/' f = self.sk_mapping[self.inc_sk.uuid]['frame'] if f < 10: f_str = '0000'+str(f) elif f < 100: f_str = '000'+str(f) elif f < 1000: f_str = '00'+str(f) elif f < 10000: f_str = '0'+str(f) elif f < 100000: f_str = str(f) # save rgb image # todo: make these rosbags sometime in the future cv2.imwrite(d+'rgb/rgb_'+f_str+'.jpg', self.rgb) cv2.imwrite(d+'depth/depth_'+f_str+'.jpg', self.xtion_img_d_rgb) cv2.imwrite(d+'rgb_sk/sk_'+f_str+'.jpg', self.rgb_sk) try: self.bag_file.write('rgb', self.rgb_msg) self.bag_file.write('depth', self.depth_msg) self.bag_file.write('rgb_sk', self.rgb_sk_msg) except: rospy.logwarn("Can not write rgb, depth, and rgb_sk to a bag file.") # save robot_pose in bag file x=float(self.robot_pose.position.x) y=float(self.robot_pose.position.y) z=float(self.robot_pose.position.z) xo=float(self.robot_pose.orientation.x) yo=float(self.robot_pose.orientation.y) zo=float(self.robot_pose.orientation.z) wo=float(self.robot_pose.orientation.w) p = Point(x, y, z) q = Quaternion(xo, yo, zo, wo) robot = Pose(p,q) self.bag_file.write('robot', robot) # save robot_pose in text file f1 = open(d+'robot/robot_'+f_str+'.txt','w') f1.write('position\n') f1.write('x:'+str(x)+'\n') f1.write('y:'+str(y)+'\n') f1.write('z:'+str(z)+'\n') f1.write('orientation\n') f1.write('x:'+str(xo)+'\n') f1.write('y:'+str(yo)+'\n') f1.write('z:'+str(zo)+'\n') f1.write('w:'+str(wo)+'\n') f1.close() # save skeleton data in bag file #x=float(self.robot_pose.position.x) #y=float(self.robot_pose.position.y) #z=float(self.robot_pose.position.z) #xo=float(self.robot_pose.orientation.x) #yo=float(self.robot_pose.orientation.y) #zo=float(self.robot_pose.orientation.z) #wo=float(self.robot_pose.orientation.w) #p = Point(x, y, z) #q = Quaternion(xo, yo, zo, wo) #skel = Pose(p,q) #bag.write('skeleton', skel) # save skeleton datain text file f1 = open(d+'skeleton/skl_'+f_str+'.txt','w') # print self.inc_sk.joints[0] f1.write('time:'+str(self.inc_sk.joints[0].time.secs)+'.'+str(self.inc_sk.joints[0].time.nsecs)+'\n') for i in self.inc_sk.joints: f1.write(i.name+'\n') f1.write('position\n') f1.write('x:'+str(i.pose.position.x)+'\n') f1.write('y:'+str(i.pose.position.y)+'\n') f1.write('z:'+str(i.pose.position.z)+'\n') f1.write('orientation\n') f1.write('x:'+str(i.pose.orientation.x)+'\n') f1.write('y:'+str(i.pose.orientation.y)+'\n') f1.write('z:'+str(i.pose.orientation.z)+'\n') f1.write('w:'+str(i.pose.orientation.w)+'\n') f1.close() # update frame number if self.inc_sk.uuid in self.sk_mapping: self.sk_mapping[self.inc_sk.uuid]['frame'] += 1 #publish the gaze request of person on every detection: if self.inc_sk.joints[0].name == 'head': head = Header(frame_id='head_xtion_depth_optical_frame') look_at_pose = PoseStamped(header = head, pose=self.inc_sk.joints[0].pose) self.publish_consent_pose.publish(look_at_pose) #self.gazeClient.send_goal(self.gazegoal) # all this should happen given a good number of detections: print "%s out of %d frames are obtained" % (self.sk_mapping[self.inc_sk.uuid]['frame'], self.after_a_number_of_frames) if self.sk_mapping[self.inc_sk.uuid]['frame'] == self.after_a_number_of_frames and self.request_sent_flag == 0: print "storing the %sth image to mongo for the webserver..." % self.after_a_number_of_frames # Skeleton on white background query = {"_meta.image_type": "white_sk_image"} white_sk_to_mongo = self.msg_store.update(message=self.white_sk_msg, meta={'image_type':"white_sk_image"}, message_query=query, upsert=True) # Skeleton on rgb background query = {"_meta.image_type": "rgb_sk_image"} rgb_sk_img_to_mongo = self.msg_store.update(message=self.rgb_sk_msg, meta={'image_type':"rgb_sk_image"}, message_query=query, upsert=True) # Skeleton on depth background query = {"_meta.image_type": "depth_image"} depth_img_to_mongo = self.msg_store.update(message=self.depth_msg, meta={'image_type':"depth_image"}, message_query=query, upsert=True) consent_msg = "Check_consent_%s" % (t) print consent_msg # I think this should be a service call - so it definetly returns a value. self.request_sent_flag = 1 # move and speak: (if no target waypoint, go to original waypoint) # self.reset_ptu() try: self.navgoal.target = self.config[waypoint]['target'] except: self.navgoal.target = waypoint if self.navgoal.target != waypoint: self.nav_goal_waypoint = waypoint #to return to after consent self.navClient.send_goal(self.navgoal) result = self.navClient.wait_for_result() if not result: self.go_back_to_where_I_came_from() self.publish_consent_req.publish(consent_msg) rospy.sleep(0.1) if self.request_sent_flag: self.speaker.send_goal(maryttsGoal(text=self.speech)) while self.consent_ret is None: rospy.sleep(0.1) # Move Eyes - look up and down to draw attension. return self.consent_ret
def consent_ret_callback(self, msg): print "-> consent ret callback", self.request_sent_flag, msg if self.request_sent_flag != 1: return self.consent_ret = msg self.speaker.send_goal(maryttsGoal(text="Thank you"))
def execute_cb(self, goal): self.listen_to_robot_pose = 1 rospy.sleep(1) duration = goal.duration start = rospy.Time.now() end = rospy.Time.now() consent_msg = "nothing" rospy.loginfo("Goal: Dur: %s ROI: %s" % (goal.duration.secs, goal.roi_id)) # Obtains the Robot's region - False if no roi. if not self.get_robot_roi(): return self._as.set_preempted() # Obtains the specified ROI to observe - if none, use robot's roi observe_polygon = self.get_roi_to_observe(goal.roi_id, goal.roi_config) if observe_polygon == None: return self._as.set_preempted() if not self.get_soma_objects(observe_polygon): return self._as.set_preempted() self.create_possible_navgoals() self.generate_viewpoints() nav_succ, rem_duration = self.goto(duration) self.sk_publisher.max_num_frames = self.number_of_frames_before_consent_needed #while (end - start).secs < rem_duration.secs: # and request_consent == 0: consented_uuid = "" request_consent = 0 if nav_succ: rospy.loginfo("init recording page and skeleton pub") self.signal_start_of_recording() self.sk_publisher.reinisialise() while (end - start).secs < rem_duration.secs and request_consent == 0: if self._as.is_preempt_requested(): break # print "1 ", self.sk_publisher.accumulate_data.keys() 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: head = Header(frame_id='head_xtion_depth_optical_frame') look_at_pose = PoseStamped( header=head, pose=incr_msgs[-1].joints[0].pose ) # Pose(position = Point(-0.4, -.3, 0))) self.publish_consent_pose.publish(look_at_pose) # 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.upload_images_to_mongo( uuid) # uploads latest images to mongo rospy.loginfo("breaking loop for: %s" % consented_uuid) #self.reset_ptu() # ferdi doesnt want the ptu resetting here self.speaker.send_goal(maryttsGoal(text=self.speech)) new_duration = rem_duration.secs - (end - start).secs consent_msg = self.consent_client(new_duration) rospy.loginfo("consent returned: %s: %s" % (consent_msg, consented_uuid)) # break end = rospy.Time.now() # after the action reset ptu and stop publisher if nav_succ: rospy.loginfo("exited loop. consent=%s" % consent_msg) # LOG THE STATS TO MONGO res = (request_consent, consent_msg) self.log_view_info(res, nav_succ, goal.roi_config, goal.roi_id, start, end) # reset everything: self.reset_everything() # 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 call_speech(self, text): self.speaker.send_goal(maryttsGoal(text=text))