|
| 1 | +""" |
| 2 | +Copyright (C) Microsoft Corporation. |
| 3 | +Copyright (C) 2025 IAMAI CONSULTING CORP |
| 4 | +MIT License. |
| 5 | +
|
| 6 | +Demonstrates controlling a JSBSim aircraft in Project AirSim with a gamepad. |
| 7 | +""" |
| 8 | + |
| 9 | +import argparse |
| 10 | +from enum import Enum |
| 11 | + |
| 12 | +from projectairsim import ProjectAirSimClient, World, Drone |
| 13 | +from projectairsim.utils import projectairsim_log |
| 14 | + |
| 15 | +try: |
| 16 | + from inputs import get_gamepad |
| 17 | +except ImportError as err: |
| 18 | + raise ImportError( |
| 19 | + "The 'inputs' package is required. Install with: pip install inputs" |
| 20 | + ) from err |
| 21 | + |
| 22 | + |
| 23 | +JOYSTICK_MIN = -32768.0 |
| 24 | +JOYSTICK_MAX = 32767.0 |
| 25 | + |
| 26 | + |
| 27 | +def normalize_signed_axis(value: int) -> float: |
| 28 | + return max(-1.0, min(1.0, float(value) / 32768.0)) |
| 29 | + |
| 30 | + |
| 31 | +def normalize_throttle(value: int) -> float: |
| 32 | + signed = normalize_signed_axis(value) |
| 33 | + return max(0.0, min(1.0, (signed + 1.0) * 0.5)) |
| 34 | + |
| 35 | + |
| 36 | +class RemoteControlReader: |
| 37 | + """Manages input from an Xbox-compatible game controller.""" |
| 38 | + |
| 39 | + def __init__(self): |
| 40 | + self.input_state = { |
| 41 | + "joystick_RH": 0, |
| 42 | + "joystick_RV": 0, |
| 43 | + "joystick_LH": 0, |
| 44 | + "joystick_LV": 0, |
| 45 | + } |
| 46 | + |
| 47 | + def read(self): |
| 48 | + updated_input = False |
| 49 | + |
| 50 | + while not updated_input: |
| 51 | + events = get_gamepad() |
| 52 | + for event in events: |
| 53 | + recognized_event = True |
| 54 | + if event.ev_type == "Absolute": |
| 55 | + if event.code == "ABS_X": |
| 56 | + self.input_state["joystick_LH"] = event.state |
| 57 | + elif event.code == "ABS_Y": |
| 58 | + self.input_state["joystick_LV"] = event.state |
| 59 | + elif event.code == "ABS_RX": |
| 60 | + self.input_state["joystick_RH"] = event.state |
| 61 | + elif event.code == "ABS_RY": |
| 62 | + self.input_state["joystick_RV"] = event.state |
| 63 | + else: |
| 64 | + recognized_event = False |
| 65 | + else: |
| 66 | + recognized_event = False |
| 67 | + |
| 68 | + if recognized_event: |
| 69 | + updated_input = True |
| 70 | + |
| 71 | + return self.input_state |
| 72 | + |
| 73 | + |
| 74 | +class ControlType(Enum): |
| 75 | + RPY = "mode1" # Roll, Pitch, Yaw |
| 76 | + TCS = "mode2" # Turn rate, climb rate, speed |
| 77 | + |
| 78 | + |
| 79 | +def apply_rpy_mode(drone: Drone, input_state): |
| 80 | + throttle = normalize_throttle(input_state["joystick_RV"]) |
| 81 | + roll = normalize_signed_axis(input_state["joystick_RH"]) |
| 82 | + pitch = normalize_signed_axis(input_state["joystick_LV"]) |
| 83 | + yaw = normalize_signed_axis(input_state["joystick_LH"]) |
| 84 | + |
| 85 | + drone.set_jsbsim_property("fcs/throttle-cmd-norm[0]", throttle) |
| 86 | + drone.set_jsbsim_property("fcs/throttle-cmd-norm[1]", throttle) |
| 87 | + drone.set_jsbsim_property("fcs/aileron-cmd-norm", roll) |
| 88 | + drone.set_jsbsim_property("fcs/elevator-cmd-norm", pitch) |
| 89 | + drone.set_jsbsim_property("fcs/rudder-cmd-norm", yaw) |
| 90 | + |
| 91 | + |
| 92 | +def apply_tcs_mode(drone: Drone, input_state): |
| 93 | + throttle = normalize_throttle(input_state["joystick_RV"]) |
| 94 | + turn_cmd = normalize_signed_axis(input_state["joystick_LH"]) |
| 95 | + climb_cmd = normalize_signed_axis(input_state["joystick_LV"]) |
| 96 | + |
| 97 | + drone.set_jsbsim_property("fcs/throttle-cmd-norm[0]", throttle) |
| 98 | + drone.set_jsbsim_property("fcs/throttle-cmd-norm[1]", throttle) |
| 99 | + drone.set_jsbsim_property("ap/heading-comm", turn_cmd) |
| 100 | + drone.set_jsbsim_property("ap/climb-rate-cmd", climb_cmd) |
| 101 | + |
| 102 | + |
| 103 | +def control_loop( |
| 104 | + drone: Drone, |
| 105 | + user_mode: ControlType, |
| 106 | + rc_reader: RemoteControlReader, |
| 107 | + print_rc: bool = True, |
| 108 | +): |
| 109 | + input_state = rc_reader.read() |
| 110 | + |
| 111 | + if print_rc: |
| 112 | + print( |
| 113 | + f"Left=({input_state['joystick_LH']:6n},{input_state['joystick_LV']:6n}), " |
| 114 | + f"Right=({input_state['joystick_RH']:6n},{input_state['joystick_RV']:6n}), ", |
| 115 | + end="\r", |
| 116 | + ) |
| 117 | + |
| 118 | + if user_mode == ControlType.RPY: |
| 119 | + apply_rpy_mode(drone, input_state) |
| 120 | + elif user_mode == ControlType.TCS: |
| 121 | + apply_tcs_mode(drone, input_state) |
| 122 | + |
| 123 | + |
| 124 | +def parse_args(): |
| 125 | + parser = argparse.ArgumentParser( |
| 126 | + description="Controla un avión JSBSim en ProjectAirSim usando gamepad." |
| 127 | + ) |
| 128 | + parser.add_argument( |
| 129 | + "mode", |
| 130 | + nargs="?", |
| 131 | + choices=[ControlType.RPY.value, ControlType.TCS.value], |
| 132 | + default=ControlType.RPY.value, |
| 133 | + help="mode1=RPY, mode2=TCS", |
| 134 | + ) |
| 135 | + parser.add_argument("--address", type=str, default="127.0.0.1") |
| 136 | + parser.add_argument("--topicsport", type=int, default=8989) |
| 137 | + parser.add_argument("--servicesport", type=int, default=8990) |
| 138 | + parser.add_argument("--sceneconfigfile", type=str, default="scene_cessna310_rc.jsonc") |
| 139 | + parser.add_argument("--simconfigpath", type=str, default="sim_config/") |
| 140 | + parser.add_argument("--robot", type=str, default="c310") |
| 141 | + parser.add_argument("--delay", type=int, default=2) |
| 142 | + return parser.parse_args() |
| 143 | + |
| 144 | + |
| 145 | +def main(): |
| 146 | + args = parse_args() |
| 147 | + user_mode = ControlType(args.mode) |
| 148 | + projectairsim_log().info(f"Control mode selected: {user_mode.value}") |
| 149 | + |
| 150 | + client = ProjectAirSimClient( |
| 151 | + address=args.address, |
| 152 | + port_topics=args.topicsport, |
| 153 | + port_services=args.servicesport, |
| 154 | + ) |
| 155 | + rc_reader = RemoteControlReader() |
| 156 | + |
| 157 | + try: |
| 158 | + client.connect() |
| 159 | + world = World( |
| 160 | + client=client, |
| 161 | + scene_config_name=args.sceneconfigfile, |
| 162 | + delay_after_load_sec=args.delay, |
| 163 | + sim_config_path=args.simconfigpath, |
| 164 | + ) |
| 165 | + drone = Drone(client, world, args.robot) |
| 166 | + |
| 167 | + drone.enable_api_control() |
| 168 | + |
| 169 | + while True: |
| 170 | + control_loop(drone, user_mode, rc_reader) |
| 171 | + |
| 172 | + except KeyboardInterrupt: |
| 173 | + print("\nExiting...") |
| 174 | + finally: |
| 175 | + try: |
| 176 | + drone.disable_api_control() |
| 177 | + except Exception: |
| 178 | + pass |
| 179 | + client.disconnect() |
| 180 | + |
| 181 | + |
| 182 | +if __name__ == "__main__": |
| 183 | + main() |
0 commit comments