def loadMapNRobot(self,mpath,rradius=0.35,gridres=0.10,idist=0.5,maxdist=1): self.robotradius=rradius self.vstp=vstpPY.VSTP() self.gridres=gridres self.idealdist=idist self.maxdist=maxdist self.vstp.init(rradius,gridres,idist,maxdist) self.mapsegs = self.vstp.loadMap(mpath) self.computeMapLimits() self.raciox=(self.displayx-5)/(self.mapmaxx-self.mapminx) self.racioy=(self.displayy-5)/(self.mapmaxy-self.mapminy)
def vstpFunc(self,iniX,iniY,goalx,goaly): #coordenadas de partida (inicio) print "odomx: %f odomy: %f" % (iniX,iniY) #coordenadas de destino (fim) print "goalx: %f goaly: %f" % (goaly,goaly) v=vstpPY.VSTP() #criacao do objeto v.init(self.robotRadius,self.gridResolution,self.idealDist,self.maxDist) self.mapsegs = v.loadMap(MAP) self.computeMapLimits(); self.traj_points =v.planTrajectory(iniX,iniY,goalx,goaly,True) #chamada a criacao do thread do mapa #self.toThread() self.trajDivider()
def __init__(self, robotRadius=0.35, gridResolution=0.1, idealDist=0.5, maxDist=1): ''' self.robotRadius=robotRadius self.gridResolution=gridResolution self.idealDist=idealDist self.maxDist=maxDist ''' self.new_traj = list() self.aux_traj = Point() #self.odom_sub = rospy.Subscriber('/turtle1/pose', Pose, self.callbackOdom) #self.danger_sub = rospy.Subscriber('/sonarFlag', Bool, self.callbackSonar) #para o segway #self.odom_sub = rospy.Subscriber('/segway_rmp_node/odom', Odometry, self.callbackOdom) self.odom_sub = rospy.Subscriber('/odomUpdater', Pose2D, self.callbackFalseOdom) self.new_odom_sub = rospy.Subscriber('/new_odom', Pose2D, self.callbackOdom) #definicao do servico para receber obstaculos no caminho self.s = rospy.Service('add_obstacle', add_obstacle, self.def_add_obstacle) self.v = vstpPY.VSTP() #criacao do objeto self.v.init(robotRadius, gridResolution, idealDist, maxDist) self.mapsegs = self.v.loadMap(MAP) self.pose = Pose2D() self.trueodomX = 0 self.trueodomY = 0 self.trueodomTheta = 0 self.falseodomX = 0 self.falseodomY = 0 self.falseodomTheta = 0 self.state = 0 self.traj_points = []