def costmap2local(self, data): print(data.info) VerticalPose = Vec2d(self.carPose.y, -self.carPose.x) * data.info.resolution HorizonPose = Vec2d(self.carPose.x, self.carPose.y) * data.info.resolution LUPosition = self.carPosition + HorizonPose * 0.5 * self.size - VerticalPose * 0.5 * self.size currentPosition = Vec2d(LUPosition.x, LUPosition.y) # notice x,y is not row and col but the axis,the map's x is car's init pose.x # so if image is from top to buttom and left to right from car's vision # in the data.data we must read from xright to xleft and yup to ydown for i in range(self.size - 1, -1, -1): for j in range(self.size): mapPosition = self.toMapPosition(currentPosition, data.info) if mapPosition.x < 0 or mapPosition.x >= data.info.width or \ mapPosition.y < 0 or mapPosition.x >= data.info.height: self.map_image[i, j, 0] = 1.0 else: self.map_image[i, j, 0] = self.toColor( data.data[mapPosition.x + mapPosition.y * data.info.width]) #move to next position currentPosition = currentPosition + VerticalPose currentPosition = LUPosition - HorizonPose * i self.env.setState(self.getData(compression=5), 4)
def trans2robotTf(self, location): carX, carY, goalX, goalY = location carPose = Vec2d(self.env.initPose[0, 0], self.env.initPose[0, 1]) carPosition = Vec2d(carX, carY) goalPosition = Vec2d(goalX, goalY) worldPose = Vec2d(1, 0) #world's x axis GC = goalPosition - carPosition costheta = worldPose.dot(carPose) sintheta = carPose.cross(worldPose) rotate = np.mat([[costheta, -sintheta], [sintheta, costheta]]) goal = np.mat([[GC.x], [GC.y]]) robotTfGoal = rotate * goal return carX, carY, robotTfGoal[0, 0], robotTfGoal[1, 0]
def __init__( self, env, size=200, ): #rospy.init_node('costmapTransform', anonymous=True) subCostmap = rospy.Subscriber("/map", OccupancyGrid, self.costmap2local) self.carPose = Vec2d(1, 0) self.carPosition = Vec2d(0, 0) self.initPose = np.mat([[1, 0, 0]]) self.map_image = np.zeros((size, size, 1)) self.size = size self.env = env
def getPosition(data): #print("receive position:{} {}".format(data.pose.pose.position.x,data.pose.pose.position.y)) position.x = data.pose.pose.position.x position.y = data.pose.pose.position.y x, y, z, w = data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w rotateMat = np.mat([[ 1 - 2 * y * y - 2 * z * z, 2 * x * y - 2 * w * z, 2 * w * y + 2 * x * z ], [ 2 * x * y + 2 * w * z, 1 - 2 * x * x - 2 * z * z, 2 * y * z - 2 * w * x ], [ 2 * x * z - 2 * w * y, 2 * w * x + 2 * y * z, 1 - 2 * x * x - 2 * y * y ]]) pose_ = initPose * rotateMat pose2d = Vec2d(pose_[0, 0], pose_[0, 1]).norm() pose.x = pose2d.x pose.y = pose2d.y gridMap.setPosition(position, pose) if receiveGoal: d = goal - pose distance = d.length() d = d.norm() sintheta = pose.cross(d) costheta = d.dot(pose) if sintheta > 0: theta = 2 * math.pi - math.acos(costheta) else: theta = math.acos(costheta) env.setState(distance, 0) env.setState(theta, 1)
def costmap2local(self, data): #print(data.header) VerticalPose = Vec2d(self.pose.y, -self.pose.x) * data.info.resolution HorizonPose = Vec2d(self.pose.x, self.pose.y) * data.info.resolution LDPosition = self.position - HorizonPose * 0.5 * self.size - VerticalPose * 0.5 * self.size currentPosition = Vec2d(LDPosition.x, LDPosition.y) # notice x,y is not row and col but the axis,the map's x is car's init pose.x # so if image is from top to buttom and left to right from car's vision # in the data.data we must read from xright to xleft and yup to ydown map_image = np.zeros((self.size, self.size, 1)) for i in range(self.size): for j in range(self.size): mapPosition = self.toMapPosition(currentPosition, data.info) if mapPosition.x < 0 or mapPosition.x >= data.info.width or \ mapPosition.y < 0 or mapPosition.x >= data.info.height: map_image[i, j, 0] = 1.0 else: map_image[i, j, 0] = self.toColor( data.data[mapPosition.x + mapPosition.y * data.info.width]) #move to next position currentPosition = currentPosition + VerticalPose currentPosition = LDPosition + HorizonPose * i LDPosition = self.position - HorizonPose * self.size - VerticalPose * self.size currentPosition = Vec2d(LDPosition.x, LDPosition.y) #self.globalMap = np.zeros((self.size * 2, self.size * 2, 1)) #print(self.globalMap.shape) #print("wtf") #t=input("wtf") #for i in range(self.size*2): #for j in range(self.size*2): #mapPosition = self.toMapPosition(currentPosition, data.info) #if mapPosition.x < 0 or mapPosition.x >= data.info.width or \ #mapPosition.y < 0 or mapPosition.x >= data.info.height: #self.globalMap[i, j, 0] = 1.0 #else: #self.globalMap[i, j, 0] = self.toColor(data.data[mapPosition.x+mapPosition.y*data.info.width]) #move to next position #currentPosition = currentPosition + VerticalPose #currentPosition = LDPosition + HorizonPose*i #self.globalMap_ = self.compressMap(self.globalMap, compression=1) self.setState(self.compressMap(map_image, compression=5), 4)
def __init__(self, name, state_n=5, action_n=3, mapsize=100): self.name = name self.action_n = action_n self.state_n = state_n self.state = [None] * 5 self.size = mapsize self.action_list = [(0.2, -0.2), (0.5, 0), (0.2, 0.2)] #(0, 0)] self.velocity = 0 self.rad = 0 self.state[2] = self.velocity self.state[3] = self.rad self.oldDistance = None self.oldV = 0 self.oldR = 0 self.distanceLimit = 1.0 * 20 self.reward = 200 self.collision = False self.initPose = np.mat([[1, 0, 0]]) self.pose = Vec2d() self.goal = Vec2d() self.position = Vec2d() self.receiveGoal = False self.globalMap = np.zeros((self.size * 2, self.size * 2, 1)) self.globalMap_ = np.zeros((self.size, self.size, 1)) self.pubAction = rospy.Publisher('/' + name + '/cmd_vel', Twist, queue_size=10) self.pubStopMovebase = rospy.Publisher('/' + name + '/move_base/cancel', GoalID, queue_size=10) self.pubGoal = rospy.Publisher('/' + name + "/move_base_simple/goal", PoseStamped, queue_size=10) self.subPosition = rospy.Subscriber('/' + name + "/odometry/filtered", Odometry, self.getPosition) self.subCollision = rospy.Subscriber('/' + name + "/scan", LaserScan, self.getCollision) self.subCostmap = rospy.Subscriber('/' + name + "/map", OccupancyGrid, self.costmap2local) self.subVel = rospy.Subscriber('/' + name + '/cmd_vel', Twist, self.getVelocity)
def getPosition(self, data): self.position.x = data.pose.pose.position.x self.position.y = data.pose.pose.position.y x, y, z, w = data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w rotateMat = np.mat([[ 1 - 2 * y * y - 2 * z * z, 2 * x * y - 2 * w * z, 2 * w * y + 2 * x * z ], [ 2 * x * y + 2 * w * z, 1 - 2 * x * x - 2 * z * z, 2 * y * z - 2 * w * x ], [ 2 * x * z - 2 * w * y, 2 * w * x + 2 * y * z, 1 - 2 * x * x - 2 * y * y ]]) pose_ = self.initPose * rotateMat.transpose() self.pose = Vec2d(pose_[0, 0], pose_[0, 1]).norm() #print("pose:{} {}".format(self.pose.x, self.pose.y)) #print("position:{} {}".format(self.position.x, self.position.y)) if self.receiveGoal: d = self.goal - self.position distance = d.length() * 20 d = d.norm() sintheta = self.pose.cross(d) try: costheta = min(max(d.dot(self.pose), -1.0), 1.0) if sintheta > 0: theta = 2 * math.pi - math.acos(costheta) else: theta = math.acos(costheta) except ValueError as e: print("value error:d:{} {} pose:{} {}".format( d.x, d.y, pose.x, pose.y)) #print("distance:{}theta:{}".format(distance,theta)) self.setState(distance, 0) if self.oldDistance is None: self.oldDistance = distance self.setState(theta, 1)
MAX_EPISODE = 3 DISPLAY_REWARD_THRESHOLD = 200 MAX_EP_STEPS = 500 RENDER = False GAMMA = 0.9 LR_A = 0.001 # Actor LR_C = 0.01 # Critic ENTROPY_BETA = 0.001 N_F = 5 #env.observation_space.shape[0] N_A = 4 #env.action_space.n pubAction = None pubStopMovebase = None env = Interact(5) gridMap = None goal = Vec2d() position = Vec2d() pose = Vec2d() initPose = np.mat([[1, 0, 0]]) receiveGoal = False #HOST_IP = "192.168.1.102" #USER_NAME= "zhang" #PASSWORD = "******" #REMOTE_PATH = "E:/tool" HOST_IP = "192.168.1.104" USER_NAME = "huxingyu" PASSWORD = "******" REMOTE_PATH = "~/service" class ACNet(object):
def toMapPosition(self, currentPosition, dataInfo): return Vec2d( int((currentPosition.x - dataInfo.origin.position.x) / dataInfo.resolution), int((currentPosition.y - dataInfo.origin.position.y) / dataInfo.resolution))