def convert2ROS(self):
def speed_conversion(sim_speed):
return sim_speed * 3.6
vehicle_state = VehicleState()
vehicle_state.vehicle_location.x = self.x
vehicle_state.vehicle_location.y = self.y
vehicle_state.vehicle_location.theta = self.theta
vehicle_state.length = self.length
vehicle_state.width = self.width
vehicle_state.vehicle_speed = speed_conversion(self.speed)
return vehicle_state
def getVehicleState(self, actor):
if actor == None:
return None
vehicle = VehicleState()
vehicle.vehicle_location.x = actor.get_transform().location.x
vehicle.vehicle_location.y = actor.get_transform().location.y
vehicle.vehicle_location.theta = (actor.get_transform().rotation.yaw *
np.pi / 180
) # CHECK : Changed this to radians.
vehicle.vehicle_speed = (
np.sqrt(actor.get_velocity().x**2 + actor.get_velocity().y**2 +
actor.get_velocity().z**2) * 3.6)
vehicle_bounding_box = actor.bounding_box.extent
vehicle.length = vehicle_bounding_box.x * 2
vehicle.width = vehicle_bounding_box.y * 2
return vehicle
def talker():
# initialize publishers
pub = rospy.Publisher(SIM_TOPIC_NAME, EnvironmentState, queue_size=10)
pub_rl = rospy.Publisher(RL_TOPIC_NAME, RLCommand, queue_size=10)
rospy.init_node(NODE_NAME, anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
# current Lane
lane_cur = Lane()
lane_points = []
for i in range(100):
lane_point = LanePoint()
lane_point.pose.y = 0
lane_point.pose.x = i / 10.
lane_point.pose.theta = 0
lane_point.width = 3
lane_points.append(lane_point)
lane_cur.lane = lane_points
# next Lane
lane_next = Lane()
lane_points_next = []
for i in range(100):
lane_point = LanePoint()
lane_point.pose.y = 3
lane_point.pose.x = i / 10.
lane_point.pose.theta = 0
lane_point.width = 3
lane_points_next.append(lane_point)
lane_next.lane = lane_points_next
# current vehicle
vehicle = VehicleState()
vehicle.vehicle_location.x = 2
vehicle.vehicle_location.y = 0
vehicle.vehicle_location.theta = 0
vehicle.vehicle_speed = 10
vehicle.length = 5
vehicle.width = 2
# sample enviroment state
env_state = EnvironmentState()
env_state.cur_vehicle_state = copy.copy(vehicle)
env_state.front_vehicle_state = copy.copy(vehicle)
env_state.back_vehicle_state = copy.copy(vehicle)
env_state.adjacent_lane_vehicles = [
copy.copy(vehicle), copy.copy(vehicle)
]
env_state.max_num_vehicles = 2
env_state.speed_limit = 40
env_state.current_lane = copy.copy(lane_cur)
env_state.next_lane = copy.copy(lane_next)
# sample RL command
rl_command = RLCommand()
rl_command.change_lane = 1
rospy.loginfo("Publishing")
pub.publish(env_state)
pub_rl.publish(rl_command)
rate.sleep()