def __init__(self, args):
# prius settings
self._settings = make_prius_settings(args)
# context for interacting with network
self._context = zmq.Context()
# prius's pose subscriber
self._pose_subscriber = self._context.socket(zmq.SUB)
self._pose_subscriber.connect(self._settings.prius_pose_address)
self._pose_subscriber.setsockopt(zmq.SUBSCRIBE,
self._settings.prius_pose_topic)
# prius's velocity subscriber
self._velocity_subscriber = self._context.socket(zmq.SUB)
self._velocity_subscriber.connect(
self._settings.prius_velocity_address)
self._velocity_subscriber.setsockopt(
zmq.SUBSCRIBE, self._settings.prius_velocity_topic)
# prius's collision subscriber
self._collision_subscriber = self._context.socket(zmq.SUB)
self._collision_subscriber.connect(
self._settings.prius_collision_address)
self._collision_subscriber.setsockopt(
zmq.SUBSCRIBE, self._settings.prius_collision_topic)
# prius's control publisher
self._prius_control_publisher = self._context.socket(zmq.PUB)
self._prius_control_publisher.bind(
self._settings.prius_control_address)
# world's control publisher
self._world_control_publisher = self._context.socket(zmq.PUB)
self._world_control_publisher.bind(
self._settings.world_control_address)
# msg for subscribing from simulator
self._pose = GZ_pose.Pose()
self._contacts = GZ_contacts.Contacts()
self._speed = GZ_vector3d.Vector3d()
# msg for publishing to simulator
self._control = GZ_pose.Pose(
) # position.x() for gas pedal percent, and y() for steering wheel angle(radians from -7.85 to 7.85), z() for brake pedel percent.
self._control.orientation.x = 0
self._control.orientation.y = 0
self._control.orientation.z = 0
self._control.orientation.w = 0
self._world_control = GZ_world_control.WorldControl()
self._T = False
def __init__(self, roadPath = None, vehiclePath = None, trackPath = None):
self._roadBoxes = []
self._containedRoadID = []
self._vehicleBoxes = []
self._trackPoints = []
self._mark = 0
self._pose = GZ_pose.Pose()
self._speed = GZ_vector3d.Vector3d()
self._box = Box()
self._Width = 400
self._Height = 320
self._ImgCenter_X = 10
self._ImgCenter_Y = 16
self._CarWidth = 2.2
self._CarHeight = 4.6
self._PixelDensity = 10
self._display = pygame.display.set_mode([self._Width,self._Height])
pygame.init()
pygame.display.set_caption("Motion Control using RL")
if vehiclePath is not None:
self.load_vehicle(vehiclePath)
if roadPath is not None:
self.load_road(roadPath)
if trackPath is not None:
self.load_track(trackPath)
def enjoyPrius(args):
#prius = simulator.Prius(args)
env = En.Env(args.road, args.vehicle, args.track)
pos = GZ_pose.Pose()
poses = []
with open("./map/track.point") as f:
line = f.readline()
while line:
contents = line.split('\t')
line = f.readline()
if len(contents) == 3:
pp = GZ_pose.Pose()
pp.position.x, pp.position.y, pp.orientation.x = float(
contents[0]), float(
contents[1]), float(contents[2]) + math.pi / 2
poses.append(pp)
#for i in range(0,90):
#for i in range(-90,96):
# pos.position.x = i * 0.5
# pos.position.y = random.random() * 2 - 1
# pos.position.y = 0
# pos.orientation.x = random.random()/20 - 0.025
# pos.orientation.x = 0
for pp in poses:
#pp.position.x = 45
#pp.position.y = 0
#pp.orientation.x = 0
print(env.testRender(pp))
#env.testRender(pos)
time.sleep(0.05)
#for i in range(0,90):
# pos.position.x = 42 + i/30
# pos.position.y = i / 30
# pos.orientation.x = i/90 * math.pi/2
# print str(pos)
# print(env.testRender(pos))
# #env.testRender(pos)
# time.sleep(0.05)
#time.sleep(0.5)
#while True:
# if args.mode == 0:
# prius.control_prius(args.value,-prius.pose().position.y/80,0)
# if args.mode == 1:
# prius.control_world()
# break
# time.sleep(0.1)
#while True:
#prius.control_world(False)
#time.sleep(5)
#print str(prius.pose())
#if args.mode == 0:
# #prius.control_prius(args.value,-prius.pose().position.y/80,0)
# #prius.control_prius(args.value,-prius.pose().position.y/20,0)
# prius.control_prius(args.value,0.2,0)
#if args.mode == 1:
# prius.control_world()
# break
#print prius.pose().orientation.x
#print str(prius.collisions())
#print prius.collisions().position.x
#print str(prius.velocity())
print '===================='
pass
import zmq
import sys
import google.protobuf.text_format
import msg.pose_pb2 as GZ_pose
import msg.world_control_pb2 as GZ_world_control
world_control = GZ_world_control.WorldControl()
world_control.reset.all = True
print str(world_control)
dest = "127.0.0.1"
port = 5563
context = zmq.Context()
subscriber = context.socket(zmq.SUB)
subscriber.connect("tcp://localhost:5563")
#subscriber.setsockopt(zmq.SUBSCRIBE, "prius_pose")
subscriber.setsockopt(zmq.SUBSCRIBE, "demo")
c = GZ_pose.Pose()
while True:
(topic, msg) = subscriber.recv_multipart()
c.ParseFromString(msg)
print "topic='%s' content='%s'" % (topic, str(c))
