Example #1
0
    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)
Example #8
0
    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"])
Example #9
0
 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"
Example #16
0
 def call_speech(self, text):
     self.speaker.send_goal(maryttsGoal(text=text))