From 527916c89a53b9569e1d7f644d3f385202151a73 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 15 Feb 2018 23:32:38 -0800 Subject: [PATCH] getRCData, setRCData APIs --- .../multirotor/api/MultirotorRpcLibClient.hpp | 4 +- .../controllers/DroneControllerBase.hpp | 4 ++ .../SimpleFlightDroneController.hpp | 5 +- .../multirotor/api/MultirotorRpcLibClient.cpp | 4 ++ .../controllers/DroneControllerBase.cpp | 55 ++++++++++++------- PythonClient/AirSimClient.py | 17 ++---- PythonClient/PythonClient.pyproj | 5 +- PythonClient/manual_mode_demo.py | 29 ++++++++++ 8 files changed, 87 insertions(+), 36 deletions(-) create mode 100644 PythonClient/manual_mode_demo.py diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index 165c81ce7b..fe2d6a9b1f 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -45,11 +45,13 @@ class MultirotorRpcLibClient : public RpcLibClientBase { Vector3r getPosition(); Vector3r getVelocity(); Quaternionr getOrientation(); - RCData getRCData(); GeoPoint getGpsLocation(); bool isSimulationMode(); std::string getDebugInfo(); + RCData getRCData(); + void setRCData(const RCData& rc_data); + DroneControllerBase::LandedState getLandedState(); bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp index b6be4fb187..0747f97491 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp @@ -202,6 +202,8 @@ class DroneControllerBase : public VehicleControllerBase { /// Get the current RC inputs when RC transmitter is talking to to flight controller virtual RCData getRCData() = 0; + virtual RCData estimateRCTrims(CancelableBase& cancelable_action, float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + /// Set the RC data that should be used by flight controller virtual void setRCData(const RCData& rcData) = 0; @@ -326,6 +328,8 @@ class DroneControllerBase : public VehicleControllerBase { } }; + RCData rc_data_trims_; + private: //methods float setNextPathPosition(const vector& path, const vector& path_segs, const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp index 9a042993ec..08b824d429 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp @@ -154,11 +154,12 @@ class SimpleFlightDroneController : public DroneControllerBase { virtual RCData getRCData() override { - return RCData(); + return last_rcData_; } virtual void setRCData(const RCData& rcData) override { + last_rcData_ = rcData; if (rcData.is_valid) { board_->setIsRcConnected(true); board_->setInputChannel(0, rcData.roll); //X @@ -335,6 +336,8 @@ class SimpleFlightDroneController : public DroneControllerBase { unique_ptr firmware_; VehicleParams safety_params_; + + RCData last_rcData_; }; }} //namespace diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index c152a87ab5..41ddf38faa 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -150,6 +150,10 @@ RCData MultirotorRpcLibClient::getRCData() { return static_cast(getClient())->call("getRCData").as().to(); } +void MultirotorRpcLibClient::setRCData(const RCData& rc_data) +{ + static_cast(getClient())->call("setRCData", MultirotorRpcLibAdapators::RCData(rc_data)); +} GeoPoint MultirotorRpcLibClient::getGpsLocation() { diff --git a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp index 616c55101f..c10595dd15 100644 --- a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp +++ b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp @@ -397,47 +397,62 @@ bool DroneControllerBase::setSafety(SafetyEval::SafetyViolationType enable_reaso return true; } -bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action) -{ - const float kMaxMessageAge = 10000000.0f /* 0.1 sec */, kTrimduration = 1, kMinCountForTrim = 10, kMaxTrim = 100, kMaxRCValue = 10000; - - if (duration <= 0) - return true; - //freeze the quaternion - Quaternionr starting_quaternion = getOrientation(); +RCData DroneControllerBase::estimateRCTrims(CancelableBase& cancelable_action, float trimduration, float minCountForTrim, float maxTrim) +{ + rc_data_trims_ = RCData(); //get trims - Waiter waiter_trim(getCommandPeriod(), kTrimduration); - RCData rc_data_trims; + Waiter waiter_trim(getCommandPeriod(), trimduration); uint count = 0; do { const RCData rc_data = getRCData(); - if (rc_data.timestamp > 0) { - rc_data_trims.add(rc_data); + if (rc_data.is_valid) { + rc_data_trims_.add(rc_data); count++; } } while (waiter_trim.sleep(cancelable_action) && !waiter_trim.is_timeout()); - if (count < kMinCountForTrim) - throw VehicleMoveException("Cannot compute RC trim because too few readings received"); + rc_data_trims_.is_valid = true; + + + if (count < minCountForTrim) { + rc_data_trims_.is_valid = false; + Utils::log("Cannot compute RC trim because too few readings received"); + } //take average - rc_data_trims.divideBy(static_cast(count)); - if (rc_data_trims.isAnyMoreThan(kMaxTrim)) - throw VehicleMoveException(Utils::stringf("RC trims does not seem to be valid: %s", rc_data_trims.toString().c_str())); + rc_data_trims_.divideBy(static_cast(count)); + if (rc_data_trims_.isAnyMoreThan(maxTrim)) { + rc_data_trims_.is_valid = false; + Utils::log(Utils::stringf("RC trims does not seem to be valid: %s", rc_data_trims_.toString().c_str())); + } - Utils::log(Utils::stringf("RCData Trims: %s", rc_data_trims.toString().c_str())); + Utils::log(Utils::stringf("RCData Trims: %s", rc_data_trims_.toString().c_str())); + + return rc_data_trims_; +} + +bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action) +{ + const float kMaxMessageAge = 0.1f /* 0.1 sec */, kMaxRCValue = 10000; + + if (duration <= 0) + return true; + + //freeze the quaternion + Quaternionr starting_quaternion = getOrientation(); Waiter waiter(getCommandPeriod(), duration); do { RCData rc_data = getRCData(); TTimeDelta age = clock()->elapsedSince(rc_data.timestamp); - if (age <= kMaxMessageAge) { - rc_data.subtract(rc_data_trims); + if (rc_data.is_valid && (rc_data.timestamp == 0 || age <= kMaxMessageAge)) { //if rc message timestamp is not set OR is not too old + if (rc_data_trims_.is_valid) + rc_data.subtract(rc_data_trims_); //convert RC commands to velocity vector const Vector3r vel_word(rc_data.pitch * vy_max/kMaxRCValue, rc_data.roll * vx_max/kMaxRCValue, 0); diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 3d3defbd41..9f9220daa0 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -101,18 +101,9 @@ def __init__(self, is_rate = True, yaw_or_rate = 0.0): class RCData(MsgpackMixin): timestamp = 0 - pitch = 0.0 - roll = 0.0 - throttle = 0.0 - yaw = 0.0 - switch1 = 0 - switch2 = 0 - switch3 = 0 - switch4 = 0 - switch5 = 0 - switch6 = 0 - switch7 = 0 - switch8 = 0 + pitch, roll, throttle, yaw = (0.0,)*4 #init 4 variable to to 0.0 + switch1, switch2, switch3, switch4 = (0,)*4 + switch5, switch6, switch7, switch8 = (0,)*4 is_initialized = False is_valid = False def __init__(self, timestamp = 0, pitch = 0.0, roll = 0.0, throttle = 0.0, yaw = 0.0, switch1 = 0, @@ -507,7 +498,7 @@ def hover(self): # query vehicle state - def getMultirotorState(self): + def getMultirotorState(self) -> MultirotorState: return MultirotorState.from_msgpack(self.client.call('getMultirotorState')) def getPosition(self): return Vector3r.from_msgpack(self.client.call('getPosition')) diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 698f4f511f..a710402a52 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - hello_drone.py + hello_car.py . @@ -46,6 +46,9 @@ Code + + Code + Code diff --git a/PythonClient/manual_mode_demo.py b/PythonClient/manual_mode_demo.py new file mode 100644 index 0000000000..43df8de8aa --- /dev/null +++ b/PythonClient/manual_mode_demo.py @@ -0,0 +1,29 @@ +""" +For connecting to the AirSim drone environment and testing API functionality +""" + +import os +import tempfile +import pprint + +from AirSimClient import * + + +# connect to the AirSim simulator +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +state = client.getMultirotorState() +s = pprint.pformat(state) +print("state: %s" % s) + +client.moveByManual(vx_max = 1E6, vy_max = 1E6, z_min = -1E6, duration = 1E10) +AirSimClientBase.wait_key('Manual mode is setup. Press any key to send RC data to takeoff') + +client.setRCData(rcdata = RCData(pitch = 0.0, throttle = 1.0, is_initialized = True, is_valid = True)) + +AirSimClientBase.wait_key('Set Yaw and pitch to 0.5') + +client.setRCData(rcdata = RCData(roll = 0.5, throttle = 1.0, yaw = 0.5, is_initialized = True, is_valid = True))