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))