<commit_before>import logging; logger = logging.getLogger("robot." + __name__)
logger.setLevel(logging.DEBUG)

import math

from robots.action import *
from robots.exception import UnknownFrameError, RobotError
import places

POS_EPSILON = 0.1 # Min distance in meters from origin to be considered as 'in game'

class PoseManager:
    """ A pose is for us a dict {'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw, 'frame':frame},
    ie a (x, y, z) cartesian pose in meter interpreted in a specific reference 
    frame, and a quaternion describing the orientation of the object in radians.
    
    This class helps with:
     * retrieving pose using ROS TF, SPARK or a static library of positions,
     * converting from other convention to our convention,
     * converting back to other conventions.
     
     """
    
    def __init__(self, robot):
        
        if robot.hasROS():
            self.ros = ROSPositionKeeper()
        else:
            logger.warning("Initializing the PoseManager without ROS support." +\
                           "TF transformations and conversions between" +\
                           " quaternions and euler angles won't be available.")
            self.ros = None
            
        if robot.hasPocolibs() and robot.hasmodule('spark'):
            from robots.action import genom_request
            self.spark = SPARKPositionKeeper(robot)
        else:
            logger.warning("Initializing the PoseManager without SPARK support." +\
                           "Positions of SPARK objects won't be available.")
            self.spark = None
        
    def quaternion_from_euler(self, rx, ry, rz):
        if not self.ros:
            raise RobotError("ROS support required for conversions between quaternions" +\
            " and euler angles.")
        from tf import transformations
        return transformations.quaternion_from_euler(rx, ry, rz, 'sxyz')
    
    def euler_from_quaternion(self, rx, ry, rz):
        if not self.ros:
            raise RobotError("ROS support required for conversions between quaternions" +\
            " and euler angles.")
        from tf import transformations
        return transformations.euler_from_quaternion(pose['qx'], pose['qy'], pose['qz'], pose['qw'], 'sxyz')
        
    def normalizedict(self, pose):
        if not 'x' in pose:
            pose['x'] = 0.0
        else:
            pose['x'] = float(pose['x'])
            
        if not 'y' in pose:
            pose['y'] = 0.0
        else:
            pose['y'] = float(pose['y'])
            
        if not 'z' in pose:
            pose['z'] = 0.0
        else:
            pose['z'] = float(pose['z'])
            
        if not 'qx' in pose:
            pose['qx'] = 0.0
        else:
            pose['qx'] = float(pose['qx'])
        
        if not 'qy' in pose:
            pose['qy'] = 0.0
        else:
            pose['qy'] = float(pose['qy'])
        
        if not 'qz' in pose:
            pose['qz'] = 0.0
        else:
            pose['qz'] = float(pose['qz'])
            
        if not 'qw' in pose:
            pose['qw'] = 1.0
        else:
            pose['qw'] = float(pose['qw'])
            
        if not 'frame' in pose:
            pose['frame'] = 'map'
        return pose
        
    def normalizelist(self, pose):
        if len(pose) == 3:
            x,y,z = pose
            return self.normalizedict({'x':x, 'y':y, 'z':z})

        if len(pose) == 4 and isinstance(pose[3], basestring): 
            # we assume the last arg is the frame
            x,y,z, frame = pose
            return self.normalizedict({'x':x, 'y':y, 'z':z, 'frame': frame})
            
        if len(pose) == 6:
            x,y,z, rx, ry, rz = pose
            qx,qy,qz,qw = self.quaternion_from_euler(rx, ry, rz)
            return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw})
            
        if len(pose) == 7:
            if isinstance(pose[6], basestring): 
                # we assume the last arg is the frame
                x,y,z, rx, ry, rz, frame = pose
                qx,qy,qz,qw = self.quaternion_from_euler(rx, ry, rz)
                return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw, 'frame': frame})
            else:
                x,y,z, qx, qy, qz, qw = pose
                return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw})
        
        if len(pose) == 8 and isinstance(pose[7], basestring): 
            # we assume the last arg is the frame
            x,y,z, qx, qy, qz, qw, frame = pose
            return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw, 'frame': frame})
       
        raise RobotError("Don't know what to do with pose as array %s" % pose)
    
    def normalize(self, pose):
        if isinstance(pose, list) or isinstance(pose, tuple):
            return self.normalizelist(pose)
        if isinstance(pose, dict):
            return self.normalizedict(pose)
        
        raise RobotError("normalize() takes either lists or dict as input.")
    
    def __getitem__(self, raw):
        """ Implements the PoseManager[] operator as an alias for PoseManager.get()
        """
        return self.get(raw)
        
    def get(self, raw):
        """ takes a loosly defined 'pose' as input and returns a properly formatted
        and normalized pose.
        
        Input may be:
         * a SPARK entity (either an object name of a pair (object name, part name))
         * a ROS TF frame
         * an incomplete pose dictionary
         * a list or tuple (x,y,z), (x,y,z,frame) or (z,y,z,rx,ry,rz) or (x,y,z,qx,qy,qz,qw)
        """
        
        pose = None
        
        if isinstance(raw, basestring):
            
            p = places.places()
            # Is it a known symbolic place?
            if raw in p.keys():
                return self.get(p[raw]) # normalize it
            
            #Either a SPARK object name or a ROS TF frame
            if self.ros:
                pose = self.ros.getabspose(raw)
            
            if not pose:
                #Trying with SPARK
                if self.spark:
                    pose = self.spark.getabspose(raw)
                if not pose:
                    raise UnknownFrameError("Unknown object or frame '%s'" % raw)
                else:
                    return self.normalize(pose)<commit_msg>When resolving position, first try with SPARK then ROS<commit_after>import logging; logger = logging.getLogger("robot." + __name__)
logger.setLevel(logging.DEBUG)

import math

from robots.action import *
from robots.exception import UnknownFrameError, RobotError
import places

POS_EPSILON = 0.1 # Min distance in meters from origin to be considered as 'in game'

class PoseManager:
    """ A pose is for us a dict {'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw, 'frame':frame},
    ie a (x, y, z) cartesian pose in meter interpreted in a specific reference 
    frame, and a quaternion describing the orientation of the object in radians.
    
    This class helps with:
     * retrieving pose using ROS TF, SPARK or a static library of positions,
     * converting from other convention to our convention,
     * converting back to other conventions.
     
     """
    
    def __init__(self, robot):
        
        if robot.hasROS():
            self.ros = ROSPositionKeeper()
        else:
            logger.warning("Initializing the PoseManager without ROS support." +\
                           "TF transformations and conversions between" +\
                           " quaternions and euler angles won't be available.")
            self.ros = None
            
        if robot.hasPocolibs() and robot.hasmodule('spark'):
            from robots.action import genom_request
            self.spark = SPARKPositionKeeper(robot)
        else:
            logger.warning("Initializing the PoseManager without SPARK support." +\
                           "Positions of SPARK objects won't be available.")
            self.spark = None
        
    def quaternion_from_euler(self, rx, ry, rz):
        if not self.ros:
            raise RobotError("ROS support required for conversions between quaternions" +\
            " and euler angles.")
        from tf import transformations
        return transformations.quaternion_from_euler(rx, ry, rz, 'sxyz')
    
    def euler_from_quaternion(self, rx, ry, rz):
        if not self.ros:
            raise RobotError("ROS support required for conversions between quaternions" +\
            " and euler angles.")
        from tf import transformations
        return transformations.euler_from_quaternion(pose['qx'], pose['qy'], pose['qz'], pose['qw'], 'sxyz')
        
    def normalizedict(self, pose):
        if not 'x' in pose:
            pose['x'] = 0.0
        else:
            pose['x'] = float(pose['x'])
            
        if not 'y' in pose:
            pose['y'] = 0.0
        else:
            pose['y'] = float(pose['y'])
            
        if not 'z' in pose:
            pose['z'] = 0.0
        else:
            pose['z'] = float(pose['z'])
            
        if not 'qx' in pose:
            pose['qx'] = 0.0
        else:
            pose['qx'] = float(pose['qx'])
        
        if not 'qy' in pose:
            pose['qy'] = 0.0
        else:
            pose['qy'] = float(pose['qy'])
        
        if not 'qz' in pose:
            pose['qz'] = 0.0
        else:
            pose['qz'] = float(pose['qz'])
            
        if not 'qw' in pose:
            pose['qw'] = 1.0
        else:
            pose['qw'] = float(pose['qw'])
            
        if not 'frame' in pose:
            pose['frame'] = 'map'
        return pose
        
    def normalizelist(self, pose):
        if len(pose) == 3:
            x,y,z = pose
            return self.normalizedict({'x':x, 'y':y, 'z':z})

        if len(pose) == 4 and isinstance(pose[3], basestring): 
            # we assume the last arg is the frame
            x,y,z, frame = pose
            return self.normalizedict({'x':x, 'y':y, 'z':z, 'frame': frame})
            
        if len(pose) == 6:
            x,y,z, rx, ry, rz = pose
            qx,qy,qz,qw = self.quaternion_from_euler(rx, ry, rz)
            return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw})
            
        if len(pose) == 7:
            if isinstance(pose[6], basestring): 
                # we assume the last arg is the frame
                x,y,z, rx, ry, rz, frame = pose
                qx,qy,qz,qw = self.quaternion_from_euler(rx, ry, rz)
                return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw, 'frame': frame})
            else:
                x,y,z, qx, qy, qz, qw = pose
                return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw})
        
        if len(pose) == 8 and isinstance(pose[7], basestring): 
            # we assume the last arg is the frame
            x,y,z, qx, qy, qz, qw, frame = pose
            return self.normalizedict({'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw, 'frame': frame})
       
        raise RobotError("Don't know what to do with pose as array %s" % pose)
    
    def normalize(self, pose):
        if isinstance(pose, list) or isinstance(pose, tuple):
            return self.normalizelist(pose)
        if isinstance(pose, dict):
            return self.normalizedict(pose)
        
        raise RobotError("normalize() takes either lists or dict as input.")
    
    def __getitem__(self, raw):
        """ Implements the PoseManager[] operator as an alias for PoseManager.get()
        """
        return self.get(raw)
        
    def get(self, raw):
        """ takes a loosly defined 'pose' as input and returns a properly formatted
        and normalized pose.
        
        Input may be:
         * a SPARK entity (either an object name of a pair (object name, part name))
         * a ROS TF frame
         * an incomplete pose dictionary
         * a list or tuple (x,y,z), (x,y,z,frame) or (z,y,z,rx,ry,rz) or (x,y,z,qx,qy,qz,qw)
        """
        
        pose = None
        
        if isinstance(raw, basestring):
            
            p = places.places()
            # Is it a known symbolic place?
            if raw in p.keys():
                return self.get(p[raw]) # normalize it
            
            #Either a SPARK object name or a ROS TF frame
            #Trying with SPARK
            if self.spark:
                pose = self.spark.getabspose(raw)
        
            if not pose:
                #Trying with ROS
                if self.ros:
                    pose = self.ros.getabspose(raw)
                if not pose:
                    raise UnknownFrameError("Unknown object or frame '%s'" % raw)
                else:
                    return self.normalize(pose)
{'fraction_non_alphanumeric': 0.12236416807757426, 'fraction_numerical': 0.0036940126212097893, 'mean_word_length': 1.720896147403685, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7564719', 'n_tokens_mistral': 4148, 'n_tokens_neox': 3956, 'n_words': 1196}