def test_posctl(self): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() manIn.offboard_posctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() self.rateSec.sleep() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") # prepare flight path positions = ((0, 0, 0), (2, 2, 2), (2, -2, 2), (-2, -2, 2), (2, 2, 2)) for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) # does it hold the position for Y seconds? positionHeld = True count = 0 timeout = 50 while (count < timeout): if (not self.is_at_position(2, 2, 2, 0.5)): positionHeld = False break count = count + 1 self.rate.sleep() self.assertTrue(count == timeout, "position could not be held")
def test_clients(): manual_input = ManualInput() client = DonkeyClient(address=(host, port)) client.collecting = False client.hardReset() client.initCar() client.reset() client.collecting = True client.telemetrie = [] while len(client.telemetrie) < 10: time.sleep(0.1) last_printed_index = 0 while True: if manual_input.q == 1: client.collecting = False break current_telemetrie = client.telemetrie[len(client.telemetrie) - 1] manual_input.loop(current_telemetrie.img) if (len(client.telemetrie) % 20 == 0): index = len(client.telemetrie) if last_printed_index != index: print(current_telemetrie.pos_x, ",", current_telemetrie.pos_y, ",", current_telemetrie.pos_z) last_printed_index = index if manual_input.e == 1: th1 = 0.8 if fabs(current_telemetrie.cte) > 0.5: th1 = 0.2 elif fabs(current_telemetrie.cte) > 1: th1 = 0.5 elif fabs(current_telemetrie.cte) > 2: th1 = 0.3 client.send_controls(-current_telemetrie.cte / 2.0, th1) else: client.send_controls(manual_input.st, manual_input.th) with open(telemetry_data + str(time.time_ns()) + '.pk', 'wb') as episode_file: episode_file.write(zlib.compress(pickle.dumps(client.telemetrie))) time.sleep(1.0) client.stop()
def __init__(self, vae): self.stackSize = 2 self.latent_dim = vae.zsize self.vae = vae self.client = DonkeyClient(address=(host, port)) self.action_space = spaces.Box(low=np.array([-1, -1]), high=np.array([1, 1]), dtype=np.float32) # stacked self.stackSize latest observations images encoded = latent_dim(picture) + (steering + throttle + speed) + (accel x,y,z) + gyro (x,y,z,w) self.observation_space = spaces.Box( low=0.0, high=1.0, shape=(self.stackSize, self.latent_dim + 3 + 3 + 4 + 3), dtype=np.float32) self.seed() self.index = 0 self.manual_input = None if enable_manual_input_during_training: self.manual_input = ManualInput() self.reward_accumulator = 1
def test_attctl(self): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() manIn.offboard_attctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() self.rateSec.sleep() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") # set some attitude and thrust att = PoseStamped() att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() quaternion = quaternion_from_euler(0.15, 0.15, 0) att.pose.orientation = Quaternion(*quaternion) throttle = Float64() throttle.data = 0.6 # does it cross expected boundaries in X seconds? count = 0 timeout = 120 while (count < timeout): # update timestamp for each published SP att.header.stamp = rospy.Time.now() self.pubAtt.publish(att) self.pubThr.publish(throttle) if (self.localPosition.pose.position.x > 5 and self.localPosition.pose.position.z > 5 and self.localPosition.pose.position.y < -5): break count = count + 1 self.rate.sleep() self.assertTrue(count < timeout, "took too long to cross boundaries")
def test_posctl(self): manIn = ManualInput() # arm and go into offboard manIn.arm() manIn.offboard() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") # prepare flight path positions = ( (0,0,0), (2,2,-2), (2,-2,-2), (-2,-2,-2), (2,2,-2)) # flight path assertion self.fpa = FlightPathAssertion(positions, 1, 0) self.fpa.start() for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) # does it hold the position for Y seconds? positionHeld = True count = 0 timeout = 50 while(count < timeout): if(not self.is_at_position(2, 2, -2, 0.5)): positionHeld = False break count = count + 1 self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") self.fpa.stop()
def test_manual_input(self): rospy.init_node('test_node', anonymous=True) rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) man_in = ManualInput() # Test arming man_in.arm() self.assertEquals(self.actuator_status.armed, True, "did not arm") # Test posctl man_in.posctl() self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # Test offboard man_in.offboard() self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")