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