shared_state_information.py

Go to the documentation of this file.

#!/usr/bin/python
# -*- coding: utf-8 -*-
#################################################################
##\file
#
# \note
# Copyright (c) 2011 \n
# Cardiff University \n\n
#
#################################################################
#
# \note
# Project name: Multi-Role Shadow Robotic System for Independent Living
# \note
# ROS stack name: srs
# \note
# ROS package name: srs_states
#
# \author
# Author: Renxi Qiu, email: renxi.qiu@gmail.com
#
# \date Date of creation: Oct 2011
#
# \brief
# information shared by the SRS components
#
#################################################################
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# - Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer. \n
#
# - Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution. \n
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License LGPL as
# published by the Free Software Foundation, either version 3 of the
# License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Lesser General Public License LGPL for more details.
#
# You should have received a copy of the GNU Lesser General Public
# License LGPL along with this program.
# If not, see <http://www.gnu.org/licenses/>.
#
#################################################################
# ROS imports
import os, sys

import roslib
roslib.load_manifest('srs_states')
import rospy
import smach
import threading
import tf
from std_msgs.msg import String, Bool, Int32
from geometry_msgs.msg import *


"""
current_task_info is a global shared memory for exchanging task information among statuses 

smach is slow on passing large amount of userdata. Hence they are stored under goal_structure as global variable

srs_dm_action perform one task at a time and maintain a unique session id.  
"""

listener = tf.TransformListener()

class goal_structure():   
    
    def __init__(self):
        
        #goal of the high level task, replaced by self.task_feedback 
        self.task_name =""
        
        #task parameter, replaced by self.task_feedback 
        self.task_parameter=""
        
        
        # contain task_id, task_initializer, task_initializer_type, task_name, task_parameter, task_schedule, action_object, action_object_parent
        # defined in srs_decision_making json_parser.py
        self.task_feedback=""
        
        
        ## by Ze
        #self.parameters = list()
        #complete json parameters come from user
        self.json_parameters = ''
        
        #Information about last step, use Last_step_info_msg 
        self.last_step_info = list()
        
        #customised pre-empty signal received or not
        self.preemptied = False
        
        #reference to the action server
        self._srs_as=""
        
        ## backward compatible need to be revised after the integration meeting         
        #feedback publisher, intervention required
        self.pub_fb = rospy.Publisher('fb_executing_solution', Bool)
        #feedback publisher, operational state
        self.pub_fb2 = rospy.Publisher('fb_executing_state', String)
        ## backward compatible need to be revised after the integration meeting  
        
        self.object_in_hand = False
        
        self.object_identified = False
        
        #True already adjusted, False not adjusted yet
        self.post_grasp_adjusted = False
        
        
        # this can be replaced by the tray service with real robot
        self.object_on_tray = False
        
        self.arm_folded_ready_for_transfer = False    #arm in hold or folded position
        
        self.lock = threading.Lock()
        
        self.customised_preempt_required = False
        
        self.customised_preempt_acknowledged = False
        
        self.pause_required = False
        
        self.stop_required = False 
        
        self.stop_acknowledged = False
        
        self.language_set = ''
        
        try:
            self.language_set = rospy.get_param("/srs/language")
        except Exception, e:
            self.language_set = 'English'
        
        #try reading the parameter server
        
        self.speaking_language = dict()
        self.feedback_messages = dict()
        
        if self.language_set.lower() == 'italian':
            self.speaking_language['Grasp']= "Ora sto afferrare il "
            self.speaking_language['Search']= "Ora sto cercando il "
            self.speaking_language['Navigation']= "Ora mi sto muovendo"
            self.speaking_language['Stop']= "Ora sto fermare l attività corrente "
            self.speaking_language['Pause']= "Ora sto pausa l attività corrente"
            self.speaking_language['Resume']= "Ora sto riprendendo l'attività corrente"
            self.speaking_language['Preempted']= "Ora vengo fermato da un altro task con priorità maggiore"
            
            self.feedback_messages['sm_srs_grasp']= "Greifen Sie das Objekt"
            self.feedback_messages['sm_put_object_on_tray']= "Legen Sie das Objekt auf dem Tablett"
            self.feedback_messages['sm_enviroment_update']= "Umgebung zu aktualisieren"
            self.feedback_messages['sm_srs_detection']= "Auf der Suche nach Objekt"
            self.feedback_messages['sm_srs_navigation']= "Umzug nach Ziel"   
            self.feedback_messages['initialise']='Initialisierung Roboter'        
            
        elif self.language_set.lower()== 'german':
            self.speaking_language['Grasp']= "Jetzt bin ich Ergreifen der "
            self.speaking_language['Search']= "Jetzt suche ich die  "
            self.speaking_language['Navigation']= "Jetzt bin ich bewegt "
            self.speaking_language['Stop']= "Jetzt bin ich Anhalten der aktuellen Aufgabe "
            self.speaking_language['Pause']= "Jetzt bin ich Anhalten der aktuellen Aufgabe"
            self.speaking_language['Resume']= "Jetzt bin ich die Wiederaufnahme der aktuellen Aufgabe"
            self.speaking_language['Preempted']= "Jetzt bin ich von einer anderen Task mit höherer Priorität unterbrochen"      
            
            self.feedback_messages['sm_srs_grasp']= "Cogliere l'oggetto"
            self.feedback_messages['sm_put_object_on_tray']= "Mettere l'oggetto sul vassoio"
            self.feedback_messages['sm_enviroment_update']= "Ambiente aggiornamento"
            self.feedback_messages['sm_srs_detection']= "Ricerca di oggetti"
            self.feedback_messages['sm_srs_navigation']= "Spostamento di destinazione"
            self.feedback_messages['initialise']='inizializzazione robot' 
            
        else:
            #default language is English 
            self.speaking_language['Grasp']= "Now I am grasping the "
            self.speaking_language['Search']= "Now I am searching the "
            self.speaking_language['Navigation']= "Now I am moving "
            self.speaking_language['Stop']= "Now I am stopping the current task "
            self.speaking_language['Pause']= "Now I am pausing the current task "
            self.speaking_language['Resume']= "Now I am resuming the current task "
            self.speaking_language['Preempt']= "Now I am stopped by another task with higher priority"
        
            self.feedback_messages['sm_srs_grasp']= "Grasping the object"
            self.feedback_messages['sm_put_object_on_tray']= "Put the object on the tray"
            self.feedback_messages['sm_enviroment_update']= "Environment update"
            self.feedback_messages['sm_srs_detection']= "Searching for object"
            self.feedback_messages['sm_srs_navigation']= "Moving to target"
            self.feedback_messages['initialise']='initialising robot' 

        
        
    def get_post_grasp_adjustment_state(self):
        self.lock.acquire()
        #True already adjusted, False not adjusted yet
        value = self.post_grasp_adjusted
        self.lock.release()
        return value 
    
    def set_post_grasp_adjustment_state(self, value):
        self.lock.acquire()
        #True already adjusted, False not adjusted yet
        self.post_grasp_adjusted = value
        self.lock.release()

    def get_object_identification_state(self):
        self.lock.acquire()
        value = self.object_identified
        self.lock.release()
        return value 
    
    def set_object_identification_state(self, value):
        self.lock.acquire()
        self.object_identified = value
        self.lock.release()
    

    def get_pause_required(self):
        self.lock.acquire()
        value = self.pause_required
        self.lock.release()
        return value 
    
    def get_customised_preempt_required(self):
        self.lock.acquire()
        value = self.customised_preempt_required
        self.lock.release()
        return value 
    
    def get_customised_preempt_acknowledged(self):
        self.lock.acquire()
        value = self.customised_preempt_acknowledged
        self.lock.release()
        return value 
        
    def get_stop_required(self):
        self.lock.acquire()
        value = self.stop_required
        self.lock.release()
        return value 

    def get_stop_acknowledged(self):
        self.lock.acquire()
        value = self.stop_acknowledged
        self.lock.release()
        return value 
    
    def set_pause_required(self,value):
        self.lock.acquire()
        self.pause_required = value
        self.lock.release()

    def set_customised_preempt_required(self,value):
        self.lock.acquire()
        self.customised_preempt_required = value
        self.lock.release()

    
    def set_customised_preempt_acknowledged(self, value):
        self.lock.acquire()
        self.customised_preempt_acknowledged = value
        self.lock.release()

        
    def set_stop_required(self, value):
        self.lock.acquire()
        self.stop_required = value
        self.lock.release()

    def set_stop_acknowledged(self, value):
        self.lock.acquire()
        self.acknowledged = value
        self.lock.release()
    
    #checking if the current atomic operation can be stopped or not
    #true can be stopped in the middle
    def stopable(self):
        #List of conditions which robot should not be stopped in the middle
        self.lock.acquire()
        #condition 1:
        #If a object has been grasped, operation should not be stopped until the object is released or arm is folded ready for transfer
        if self.object_in_hand and not self.arm_folded_ready_for_transfer:
            outcome = False
        else:
            outcome = True
        self.lock.release()
        
        return outcome

    
    def reset(self):
        
        self.__init__()
        gc.collect()
        
    def get_robot_pos(self):
        try:
            #listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0))
            (trans,rot) = listener.lookupTransform("/map", "/base_link", rospy.Time(0))
        except rospy.ROSException, e:
            print "Transformation not possible: %s"%e
            return None
            
        rb_pose = Pose2D()
        rb_pose.x = trans[0]
        rb_pose.y = trans[1]
        rb_pose_rpy = tf.transformations.euler_from_quaternion(rot)
        rb_pose.theta = rb_pose_rpy[2]
        
        return rb_pose

    

current_task_info = goal_structure()