findGoal.py

Go to the documentation of this file.

#!/usr/bin/env python

import sys
import roslib; roslib.load_manifest('ar_kinect')
import rospy

import tf
from geometry_msgs.msg import Pose2D, PoseStamped, Pose, PoseWithCovarianceStamped
from nav_msgs.msg import OccupancyGrid, MapMetaData
from ar_pose.msg import ARMarkers
from math import sin, cos, pi
import numpy
import array
from std_msgs.msg import Float64
from tf import TransformListener
import copy
import shlex

change = 0

class Obstacles:
    def __init__(self):
        global change
        liste = []
        self.markers = []
        
        fobj = open("/ros_workspace/robin-jku-linz-ros-pkg/biped_footstep_planner/data/objects")
        self.numOfMarker = int(fobj.readlines()[0])
        fobj.close()

        obj = open("/ros_workspace/robin-jku-linz-ros-pkg/biped_footstep_planner/data/objects")

        i = 4  
        line = obj.readlines()[i:i+self.numOfMarker]
        liste.append(line)

        j = 0
        while(j < self.numOfMarker):
            ls = shlex.split(liste[0][j])
            self.markers.append(ls)
            j = j + 1

        k = 0
        self.numOfMarker = int(self.numOfMarker)
        while(k < self.numOfMarker):
            l = 1
            while(l < 9):
                if(l == 5):
                    print "TEST"
                    self.markers[k][l] = Pose2D()
                else: 
                    self.markers[k][l] = float(self.markers[k][l])
                l = l + 1
            k = k + 1

        #print self.markers
        obj.close()
        # Hindernisse werden gerade auf Marker uebertragen
        
        # setup publishers and subscribersabout:home
        rospy.Subscriber('ar_pose_markers', ARMarkers, self.localize)
       
    def localize(self, data):
        # new temporary pose
        global counter_markers
        counter_markers = 0
        global coordinates
        #Variabe change beschreibt ob ein neu zeichnen der Karte notwendig ist (Bei Aenderung der Koordinaten)
        #change = 1: Aenderung notwendig
        global change
        change = 0
        for marker in data.markers:
            if ls.frameExists(self.markers[marker.id][0]) and ls.frameExists("/map_frame"):
                t = ls.getLatestCommonTime("/map_frame", self.markers[marker.id][0])
                position, quaternion = ls.lookupTransform("/map_frame", self.markers[marker.id][0], t)
                
                x = position[0]
                y = 4-position[1]
                
                counter_markers = counter_markers + 1 
                
                self.markers[marker.id][6] = 1
                self.markers[marker.id][5].x = x
                self.markers[marker.id][5].y = y
                
                orientation = quaternion

                (phi, psi, theta) = tf.transformations.euler_from_quaternion([orientation[0],orientation[1],orientation[2],orientation[3]], axes = 'rxyz')
                #theta: Winkel von ebener Drehung (liegender Marker)
                #phi: Winkel ob Marker liegend oder stehen
                
                #print "BERECHNUNGEN"
                #print x
                #print coordinates[marker.id][0]
                #print abs(coordinates[marker.id][0]-x)
                
                if(abs(coordinates[marker.id][0]-x) >= 0.005 or abs(coordinates[marker.id][1]-y) >= 0.005 or abs(coordinates[marker.id][2]-phi) >= 0.1 or abs(coordinates[marker.id][3]-psi) >= 0.1 or abs(coordinates[marker.id][4]-theta) >= 0.1):
                    coordinates[marker.id][0] = x
                    coordinates[marker.id][1] = y
                    coordinates[marker.id][2] = phi
                    coordinates[marker.id][3] = psi
                    coordinates[marker.id][4] = theta
                    #print x, y, phi, psi, theta
                    global change
                    change = 1

                if phi > pi:
                    phi = phi - 2*pi
                elif phi < - pi:
                    phi = phi + 2*pi
                #Unterscheiden ob Marker steht oder liegt
                if (pi/4 <= phi <= 3*pi/4) or (-pi/4 >= phi >= -3*pi/4):
                    #Marker stehend
                    #print "stehend"
                    #Unterscheiden welche Grundflaechen gewaehlt werden muessen
                    if(-pi/4 <= theta <= pi/4) or (-pi <= theta <= -3*pi/4) or (pi >= theta >= 3*pi/4):
                        #Marker steht richtig oder um 180 Grad gedreht
                        #Grundflaeche: Laenge und Hoehe
                        self.markers[marker.id][8] = 1
                    else:
                        #Grundflaeche: Breite und Hoehe
                        self.markers[marker.id][8] = 2
                        
                    self.markers[marker.id][5].theta = -psi
                    
                else:
                    #Marker liegend
                    #print "liegend"
                    #Grundflaeche: Laenge und Breite
                    self.markers[marker.id][5].theta = -theta
                
class Rectangle:
    def __init__(self):
        self.corners = { 0: Pose2D(), 1: Pose2D(), 2: Pose2D(), 3: Pose2D() }
        self.width = 0
        self.height = 0
        self.rotation = 0
        
    def __init__(self, pose, width, height):
        #Knoten 0 ist der Eckknoten links oben im ungedrehten Zustand
        #Durchnummeriert in mathematisch positiver Richtung

        if pose.theta!=0:
            corner_lu = Pose2D(pose.x, pose.y, 0)
            #print "x"
            #print pose.x
            #print "y"
            #print pose.y
            corner_ld = Pose2D(pose.x+height*sin(pose.theta), pose.y-height*cos(pose.theta), 0)
            corner_rd = Pose2D(corner_ld.x+width*cos(pose.theta), corner_ld.y+width*sin(pose.theta), 0)
            corner_ru = Pose2D(pose.x+width*cos(pose.theta), pose.y+width*sin(pose.theta), 0)
        else:
            corner_lu = Pose2D(pose.x, pose.y, 0)
            corner_ld = Pose2D(pose.x, pose.y-height, 0)
            corner_rd = Pose2D(corner_ld.x+width, corner_ld.y, 0)
            corner_ru = Pose2D(pose.x+width, pose.y, 0)
        
        self.corners = { 0: corner_lu, 1: corner_ld, 2: corner_rd, 3: corner_ru }
        self.width = width
        self.height = height
        self.rotation = pose.theta
    
    def isPointInside(self, point):
        #rotiere Punkt in Rechteck-Relativkoordinatensystem
        if self.rotation != 0:
            point_rot = Pose2D(point.x*cos(self.rotation)+point.y*sin(self.rotation),-point.x*sin(self.rotation)+point.y*cos(self.rotation), 0)
            corner_lu_rot = Pose2D(self.corners[0].x*cos(self.rotation)+self.corners[0].y*sin(self.rotation), -self.corners[0].x*sin(self.rotation)+self.corners[0].y*cos(self.rotation), 0)
        else:
            point_rot = point;
            corner_lu_rot = self.corners[0]
        
        #Pruefe ob Punkt im Rechteck liegt
        return (corner_lu_rot.x <= point_rot.x <= corner_lu_rot.x+self.width) and (corner_lu_rot.y-self.height <= point_rot.y <= corner_lu_rot.y)

    def crossWith(self, anotherRect):
        # Vorabkontrolle ob sich die Gegenstaende ueberhaupt schneiden koennen
        # Mittelpunkte der Rechtecke
        diagonal_1_x = anotherRect.corners[2].x - anotherRect.corners[0].x
        diagonal_1_y = anotherRect.corners[2].y - anotherRect.corners[0].y
        
        diagonal_2_x = self.corners[2].x - self.corners[0].x
        diagonal_2_y = self.corners[2].y - self.corners[0].y
        
        # Quadrat der Rechteckdiagonalen
        diagonal_1 = (diagonal_1_x**2+diagonal_1_y**2)**0.5
        diagonal_2 = (diagonal_2_x**2+diagonal_2_y**2)**0.5
        
        r_m1 = Pose2D(anotherRect.corners[0].x + diagonal_1_x/2,anotherRect.corners[0].y + diagonal_1_y/2,0)
        r_m2 = Pose2D(self.corners[0].x + diagonal_2_x/2,self.corners[0].y + diagonal_2_y/2,0)
        
        # Berechnen des Abstands
        if (r_m1.x-r_m2.x)**2 + (r_m1.y-r_m2.y)**2 > (diagonal_1+diagonal_2)**2:
            return False
        else:
        # print self.rotation
            selfInRect = self.isPointInside(anotherRect.corners[0]) or self.isPointInside(anotherRect.corners[1]) or self.isPointInside(anotherRect.corners[2]) or self.isPointInside(anotherRect.corners[3])
            rectInSelf = anotherRect.isPointInside(self.corners[0]) or anotherRect.isPointInside(self.corners[1]) or anotherRect.isPointInside(self.corners[2]) or anotherRect.isPointInside(self.corners[3])
            
            return rectInSelf or selfInRect

def fillobstacles(obstacles, occupancy):
    #Berechnet ob in einem Raster ein Hinderniss ist
    
    rows = xrange(len(occupancy.data))
    cols = xrange(len(occupancy.data[1]))
    
    newMap = 0
    for marker_id in xrange(obstacles.numOfMarker):
         marker = obstacles.markers[marker_id]
         
         if marker[6]==1:               
             newMap = 1
    
    if newMap == 1:
        #Neu zeichnen der Karte
        for row in rows:
            for col in cols:
                occupancy.data[row][col] = 0
    
        for marker_id in xrange(obstacles.numOfMarker):
            marker = obstacles.markers[marker_id]
            if (marker[6] == 1):
                #Berechnung Eckpunkt links oben (ungedrehte Lage) von Hindernis
                corner_lu = Pose2D(marker[5].x-marker[3]*cos(marker[5].theta)-marker[4]*sin(marker[5].theta), marker[5].y-marker[3]*sin(marker[5].theta)+marker[4]*cos(marker[5].theta),marker[5].theta)
                #Ueberpruefen wie Marker liegt, damit richtige Grundabmasse von Gegenstand fuer Karte uebergeben werden
                #Laenge = marker[1]; Breite = marker[2]; Hoehe = marker[7]
                #0: Laenge und Breite
                #1: Laenge und Hoehe
                #2: Breite und Hoehe
            
                if (marker[8] == 0):
                    obstacle = Rectangle(corner_lu, marker[1], marker[2])
                elif (marker[8] == 1):
                    obstacle = Rectangle(corner_lu, marker[1], marker[7])
                else: 
                    obstacle = Rectangle(corner_lu, marker[2], marker[7])
            
                marker[8] = 0
                                                
    
                for row in rows:
                    for col in cols:
                        #Berechnung Rastereckpunkte
                        #cornecreateOccGrid (copy)r_lu = Pose2D((col)*occupancy.info.resolution, (occupancy.info.height-row-1)*occupancy.info.resolution, 0)
                        corner_lu = Pose2D((col)*occupancy.info.resolution, (occupancy.info.height-row)*occupancy.info.resolution, 0)
            
                        raster_element = Rectangle(corner_lu, occupancy.info.resolution, occupancy.info.resolution)
                        if obstacle.crossWith(raster_element):
                            #belegt
                            occupancy.data[row][col] = 100
                marker[6] = 0
                       
    return occupancy
               

ls = TransformListener()

counter_markers = 0
     
if __name__ == '__main__':
    rospy.init_node('calcmap')
   
   
    fobj = open("/ros_workspace/robin-jku-linz-ros-pkg/biped_footstep_planner/data/objects")
    numMarker = int(fobj.readlines()[0])
    fobj.close()
    
    global coordinates
    #pro Zeile werden der Reihe nach die alten Koordinaten der Marker gespeichert
    coordinates = [[0.0000 for j in xrange(5)] for i in xrange(numMarker)]
    
    #Aktueller Winkel Kinekt-Motor
    #rospy.Subscriber('cur_tilt_angle', Float64, getAngle)
    #Erstelle Hindernisse
    obstacles = Obstacles()
    
    #Erstelle Karte 
    info = MapMetaData()
    #Angabe in Pixel
    info.width = 80
    info.height = 80
    #Meter pro Pixel/Raster
    info.resolution = 0.05

    #Belegtheitsmatrix
    occMat = [[0 for j in xrange(info.width)] for i in xrange(info.height)]
    occupancy = OccupancyGrid()
    old_occupancy = OccupancyGrid()
    old_occupancy.data = occMat[:][:]
    temp_occupancy = OccupancyGrid()
    occupancy.info = info
    occupancy.data = occMat[:][:]
    occupancy.header.frame_id = "/map_frame"
    old_occupancy.data = occMat[:][:]
    pub1 = rospy.Publisher('drawmap_coord', OccupancyGrid)
    pub2 = rospy.Publisher('drawmap_pixel', OccupancyGrid)
    
    #Kontrolle ob Knoten schon gestartet (keine 0/0 Eintraege)
    marker = obstacles.markers[2]

    r = rospy.Rate(rospy.get_param('rate', 10.0))    
    global change
    change = 1
    
    
    while not rospy.is_shutdown():
        global change                           
        
        rows = xrange(len(occupancy.data))
        cols = xrange(len(occupancy.data[1]))
                
        occupancy = fillobstacles(obstacles, occupancy)
        #print "COOOOOOOUUUUUUUNNNNNNNNTTTTTTEEEEEEERRRRRRRR Marker"
        #print counter_markers
        counter = 0

        for row in rows:
            for col in cols:
                if(old_occupancy.data[row][col] != occupancy.data[row][col]):
                    counter = counter + 1
                    
        temp_occupancy.info = info
        temp_occupancy.header.frame_id = "/map_frame"        
    
        #LEERE KARTE --> wird direkt gepublisht
        if(counter_markers == 0):
           #print "keine Marker"
           occMat = [[0 for j in xrange(info.width)] for i in xrange(info.height)]
           occupancy.data = occMat
           occMat_numpy = numpy.matrix(occupancy.data)
           occMat_list = list(numpy.array(occMat_numpy).reshape(-1,))
           occMat_int8list = array.array('i',occMat_list)
           temp_occupancy.data = occMat_int8list
           pub1.publish(temp_occupancy)
           pub2.publish(temp_occupancy)
           #print occupancy.data
        
        #occMat_numpy = numpy.matrix(old_occupancy.data)
        occMat_numpy = numpy.matrix(occupancy.data)
        occMat_list = list(numpy.array(occMat_numpy).reshape(-1,))
        occMat_int8list = array.array('i',occMat_list)
    
        temp_occupancy.data = occMat_int8list
        
        #Fuer Knoten auf Basis der Pixelelemente 
        if(counter > counter_markers*10):              
            old_occupancy.data = copy.deepcopy(occupancy.data)     
            pub2.publish(temp_occupancy)
        
        #Fuer Knoten auf Basis der Koordinaten    
        if(change == 1):   
            pub1.publish(temp_occupancy)
            change = 0          
        r.sleep()