From e6fb56ea5d8a911f5870ed846823ec36eed828c6 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 26 Apr 2018 22:56:42 -0700 Subject: [PATCH] unify reset() mechanism with consistency across car and drone, make armDisarm as promitive --- AirLib/include/api/RpcLibClientBase.hpp | 1 + AirLib/include/api/SimModeApiBase.hpp | 1 + AirLib/include/api/VehicleApiBase.hpp | 1 + .../vehicles/multirotor/api/MultirotorApi.hpp | 2 +- .../controllers/DroneControllerBase.hpp | 2 +- .../simple_flight/firmware/OffboardApi.hpp | 2 +- AirLib/src/api/RpcLibClientBase.cpp | 5 + AirLib/src/api/RpcLibServerBase.cpp | 4 +- .../multirotor/api/MultirotorRpcLibClient.cpp | 4 - .../multirotor/api/MultirotorRpcLibServer.cpp | 2 - DroneServer/main.cpp | 5 + MavLinkCom/include/MavLinkConnection.hpp | 8 +- MavLinkCom/src/MavLinkConnection.cpp | 50 ++++---- MavLinkCom/src/impl/MavLinkConnectionImpl.cpp | 12 +- MavLinkCom/src/impl/MavLinkConnectionImpl.hpp | 116 +++++++++--------- PythonClient/AirSimClient.py | 6 +- PythonClient/PythonClient.pyproj | 7 ++ PythonClient/reset_test_car.py | 25 ++++ PythonClient/reset_test_drone.py | 23 ++++ Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 18 +-- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 2 - .../Plugins/AirSim/Source/Car/CarPawnApi.cpp | 9 +- Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h | 1 + .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 18 +-- .../Source/Multirotor/MultiRotorConnector.cpp | 13 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 7 +- .../AirSim/Source/SimMode/SimModeBase.h | 1 + .../Source/SimMode/SimModeWorldBase.cpp | 5 +- .../AirSim/Source/VehiclePawnWrapper.cpp | 9 +- docs/apis.md | 2 +- docs/reinforcement_learning.md | 2 + 31 files changed, 211 insertions(+), 152 deletions(-) create mode 100644 PythonClient/reset_test_car.py create mode 100644 PythonClient/reset_test_drone.py diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index ab082bd5..7cd0e5aa 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -32,6 +32,7 @@ public: bool isApiControlEnabled(); void enableApiControl(bool is_enabled); void reset(); + bool armDisarm(bool arm); CollisionInfo getCollisionInfo(); diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp index 6a6d4b3f..b0957339 100644 --- a/AirLib/include/api/SimModeApiBase.hpp +++ b/AirLib/include/api/SimModeApiBase.hpp @@ -14,6 +14,7 @@ class SimModeApiBase { public: virtual VehicleApiBase* getVehicleApi() = 0; virtual bool isPaused() const = 0; + virtual void reset() = 0; virtual void pause(bool is_paused) = 0; virtual void continueForTime(double seconds) = 0; virtual ~SimModeApiBase() = default; diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index 6eac2663..5bb56565 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -14,6 +14,7 @@ class VehicleApiBase { public: virtual GeoPoint getHomeGeoPoint() const = 0; virtual void enableApiControl(bool is_enabled) = 0; + virtual bool armDisarm(bool arm) = 0; virtual bool isApiControlEnabled() const = 0; virtual void reset() = 0; diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index 829133c1..ea477025 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp @@ -44,7 +44,7 @@ public: } virtual ~MultirotorApi() = default; - bool armDisarm(bool arm) + virtual bool armDisarm(bool arm) override { CallLock lock(controller_, action_mutex_, cancel_mutex_, pending_); pending_ = std::make_shared(); diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp index 66c7a879..26d4e378 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp @@ -45,7 +45,7 @@ public: //types public: const SafetyEval::EvalResult result; - UnsafeMoveException(const SafetyEval::EvalResult result_val, const string message = "") + UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") : VehicleMoveException(message), result(result_val) {} }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp index 5018983a..d81f6ffa 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -44,7 +44,7 @@ public: else { if (clock_->millis() - goal_timestamp_ > params_->api_goal_timeout) { if (!is_api_timedout_) { - comm_link_->log("API call timed out, entering hover mode"); + comm_link_->log("API call was not received, entering hover mode for safety"); goal_mode_ = GoalMode::getPositionMode(); goal_ = Axis4r::xyzToAxis4(state_estimator_->getPosition(), true); is_api_timedout_ = true; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 442e6fb6..a31b124a 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -119,6 +119,11 @@ void* RpcLibClientBase::getClient() return &pimpl_->client; } +bool RpcLibClientBase::armDisarm(bool arm) +{ + return pimpl_->client.call("armDisarm", arm).as(); +} + CameraInfo RpcLibClientBase::getCameraInfo(int camera_id) { return pimpl_->client.call("getCameraInfo", camera_id).as().to(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 6d208bcc..fff57ca4 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -88,7 +88,7 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad }); pimpl_->server.bind("reset", [&]() -> void { - getVehicleApi()->reset(); + getSimModeApi()->reset(); }); pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void { @@ -111,7 +111,7 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad pimpl_->server.bind("enableApiControl", [&](bool is_enabled) -> void { getVehicleApi()->enableApiControl(is_enabled); }); pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return getVehicleApi()->isApiControlEnabled(); }); - + pimpl_->server.bind("armDisarm", [&](bool arm) -> bool { return getVehicleApi()->armDisarm(arm); }); pimpl_->server.bind("getCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { const auto& collision_info = getVehicleApi()->getCollisionInfo(); return RpcLibAdapatorsBase::CollisionInfo(collision_info); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 46df2b0c..10b29cb8 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -40,10 +40,6 @@ MultirotorRpcLibClient::MultirotorRpcLibClient(const string& ip_address, uint16 MultirotorRpcLibClient::~MultirotorRpcLibClient() {} -bool MultirotorRpcLibClient::armDisarm(bool arm) -{ - return static_cast(getClient())->call("armDisarm", arm).as(); -} void MultirotorRpcLibClient::setSimulationMode(bool is_set) { static_cast(getClient())->call("setSimulationMode", is_set); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index ca3bcba5..b9b86962 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -33,8 +33,6 @@ typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; MultirotorRpcLibServer::MultirotorRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port) : RpcLibServerBase(simmode_api, server_address, port) { - (static_cast(getServer()))-> - bind("armDisarm", [&](bool arm) -> bool { return getDroneApi()->armDisarm(arm); }); (static_cast(getServer()))-> bind("setSimulationMode", [&](bool is_set) -> void { getDroneApi()->setSimulationMode(is_set); }); (static_cast(getServer()))-> diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index b2d1ed15..ce5812aa 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -28,6 +28,11 @@ public: return api_; } + virtual void reset() override + { + throw std::domain_error("reset is not supported"); + } + virtual bool isPaused() const override { return false; diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp index ba22d8d0..c1b270bd 100644 --- a/MavLinkCom/include/MavLinkConnection.hpp +++ b/MavLinkCom/include/MavLinkConnection.hpp @@ -60,7 +60,7 @@ namespace mavlinkcom { // create connection over serial port (e.g. /dev/ttyACM0 or on windows "com5"). // pass initial string to write to the port, which can be used to configure the port. // For example, on PX4 you can send "sh /etc/init.d/rc.usb\n" to turn on lots of mavlink streams. - static std::shared_ptr connectSerial(const std::string& nodeName, std::string portName, int baudrate = 115200, const std::string initString = ""); + static std::shared_ptr connectSerial(const std::string& nodeName, const std::string& portName, int baudrate = 115200, const std::string& initString = ""); // Start listening on a specific local port for packets from any remote computer. Once a packet is received // it will remember the remote address of the sender so that subsequend sendMessage calls will go back to that sender. @@ -69,7 +69,7 @@ namespace mavlinkcom { // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet // adapter rather than 127.0.0.1. - static std::shared_ptr connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort); + static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); // Connect to a specific remote machine that is already listening on a specific port for messages from any computer. // This will use any free local port that is available. @@ -77,13 +77,13 @@ namespace mavlinkcom { // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet // adapter rather than 127.0.0.1. - static std::shared_ptr connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort); + static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); // This method sets up a tcp connection to the specified remote host and port. The remote host // must already be listening and accepting TCP socket connections for this to succeed. // The localAddr can also a specific local ip address if you need to specify which // NIC to use, for example, wifi versus hard wired ethernet adapter. For localhost pass 127.0.0.1. - static std::shared_ptr connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort); + static std::shared_ptr connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort); // instance methods std::string getName(); diff --git a/MavLinkCom/src/MavLinkConnection.cpp b/MavLinkCom/src/MavLinkConnection.cpp index 6c2d321e..2150e4a9 100644 --- a/MavLinkCom/src/MavLinkConnection.cpp +++ b/MavLinkCom/src/MavLinkConnection.cpp @@ -9,49 +9,49 @@ using namespace mavlinkcom_impl; MavLinkConnection::MavLinkConnection() { - pImpl.reset(new MavLinkConnectionImpl()); + pImpl.reset(new MavLinkConnectionImpl()); } -std::shared_ptr MavLinkConnection::connectSerial(const std::string& nodeName, std::string portName, int baudrate, const std::string initString) +std::shared_ptr MavLinkConnection::connectSerial(const std::string& nodeName, const std::string& portName, int baudrate, const std::string& initString) { - return MavLinkConnectionImpl::connectSerial(nodeName, portName, baudrate, initString); + return MavLinkConnectionImpl::connectSerial(nodeName, portName, baudrate, initString); } -std::shared_ptr MavLinkConnection::connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort) +std::shared_ptr MavLinkConnection::connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort) { - return MavLinkConnectionImpl::connectLocalUdp(nodeName, localAddr, localPort); + return MavLinkConnectionImpl::connectLocalUdp(nodeName, localAddr, localPort); } -std::shared_ptr MavLinkConnection::connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort) +std::shared_ptr MavLinkConnection::connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort) { - return MavLinkConnectionImpl::connectRemoteUdp(nodeName, localAddr, remoteAddr, remotePort); + return MavLinkConnectionImpl::connectRemoteUdp(nodeName, localAddr, remoteAddr, remotePort); } -std::shared_ptr MavLinkConnection::connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort) +std::shared_ptr MavLinkConnection::connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort) { - return MavLinkConnectionImpl::connectTcp(nodeName, localAddr, remoteIpAddr, remotePort); + return MavLinkConnectionImpl::connectTcp(nodeName, localAddr, remoteIpAddr, remotePort); } void MavLinkConnection::startListening(const std::string& nodeName, std::shared_ptr connectedPort) { - pImpl->startListening(shared_from_this(), nodeName, connectedPort); + pImpl->startListening(shared_from_this(), nodeName, connectedPort); } // log every message that is "sent" using sendMessage. void MavLinkConnection::startLoggingSendMessage(std::shared_ptr log) { - pImpl->startLoggingSendMessage(log); + pImpl->startLoggingSendMessage(log); } void MavLinkConnection::stopLoggingSendMessage() { - pImpl->stopLoggingSendMessage(); + pImpl->stopLoggingSendMessage(); } void MavLinkConnection::close() { - pImpl->close(); + pImpl->close(); } bool MavLinkConnection::isOpen() @@ -70,54 +70,54 @@ int MavLinkConnection::prepareForSending(MavLinkMessage& msg) std::string MavLinkConnection::getName() { - return pImpl->getName(); + return pImpl->getName(); } int MavLinkConnection::getTargetComponentId() { - return pImpl->getTargetComponentId(); + return pImpl->getTargetComponentId(); } int MavLinkConnection::getTargetSystemId() { - return pImpl->getTargetSystemId(); + return pImpl->getTargetSystemId(); } uint8_t MavLinkConnection::getNextSequence() { - return pImpl->getNextSequence(); + return pImpl->getNextSequence(); } void MavLinkConnection::sendMessage(const MavLinkMessageBase& msg) { - pImpl->sendMessage(msg); + pImpl->sendMessage(msg); } void MavLinkConnection::sendMessage(const MavLinkMessage& msg) { - pImpl->sendMessage(msg); + pImpl->sendMessage(msg); } int MavLinkConnection::subscribe(MessageHandler handler) { - return pImpl->subscribe(handler); + return pImpl->subscribe(handler); } void MavLinkConnection::unsubscribe(int id) { - pImpl->unsubscribe(id); + pImpl->unsubscribe(id); } MavLinkConnection::~MavLinkConnection() { - pImpl->close(); - pImpl = nullptr; + pImpl->close(); + pImpl = nullptr; } void MavLinkConnection::join(std::shared_ptr remote, bool subscribeToLeft, bool subscribeToRight) { - pImpl->join(remote, subscribeToLeft, subscribeToRight); + pImpl->join(remote, subscribeToLeft, subscribeToRight); } // get the next telemetry snapshot, then clear the internal counters and start over. This way each snapshot // gives you a picture of what happened in whatever timeslice you decide to call this method. void MavLinkConnection::getTelemetry(MavLinkTelemetry& result) { - pImpl->getTelemetry(result); + pImpl->getTelemetry(result); } diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index f52b3d25..f38d5e91 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -45,7 +45,7 @@ std::shared_ptr MavLinkConnectionImpl::createConnection(cons return con; } -std::shared_ptr MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort) +std::shared_ptr MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort) { std::shared_ptr socket = std::make_shared(); @@ -54,7 +54,7 @@ std::shared_ptr MavLinkConnectionImpl::connectLocalUdp(const return createConnection(nodeName, socket); } -std::shared_ptr MavLinkConnectionImpl::connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort) +std::shared_ptr MavLinkConnectionImpl::connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort) { std::string local = localAddr; // just a little sanity check on the local address, if remoteAddr is localhost then localAddr must be also. @@ -69,7 +69,7 @@ std::shared_ptr MavLinkConnectionImpl::connectRemoteUdp(cons return createConnection(nodeName, socket); } -std::shared_ptr MavLinkConnectionImpl::connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort) +std::shared_ptr MavLinkConnectionImpl::connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort) { std::string local = localAddr; // just a little sanity check on the local address, if remoteAddr is localhost then localAddr must be also. @@ -84,13 +84,13 @@ std::shared_ptr MavLinkConnectionImpl::connectTcp(const std: return createConnection(nodeName, socket); } -std::shared_ptr MavLinkConnectionImpl::connectSerial(const std::string& nodeName, std::string name, int baudRate, const std::string initString) +std::shared_ptr MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString) { std::shared_ptr serial = std::make_shared(); - int hr = serial->connect(name.c_str(), baudRate); + int hr = serial->connect(portName.c_str(), baudRate); if (hr != 0) - throw std::runtime_error(Utils::stringf("Could not open the serial port %s, error=%d", name.c_str(), hr)); + throw std::runtime_error(Utils::stringf("Could not open the serial port %s, error=%d", portName.c_str(), hr)); // send this right away just in case serial link is not already configured if (initString.size() > 0) { diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp index a8d9e53e..daa83dfc 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp @@ -27,77 +27,77 @@ using namespace mavlinkcom; namespace mavlinkcom_impl { - // See MavLinkConnection.hpp for definitions of these methods. - class MavLinkConnectionImpl - { - public: - MavLinkConnectionImpl(); - static std::shared_ptr connectSerial(const std::string& nodeName, std::string portName, int baudrate = 115200, const std::string initString = ""); - static std::shared_ptr connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort); - static std::shared_ptr connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort); - static std::shared_ptr connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort); + // See MavLinkConnection.hpp for definitions of these methods. + class MavLinkConnectionImpl + { + public: + MavLinkConnectionImpl(); + static std::shared_ptr connectSerial(const std::string& nodeName, const std::string& portName, int baudRate = 115200, const std::string& initString = ""); + static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); + static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); + static std::shared_ptr connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort); - std::string getName(); - int getTargetComponentId(); - int getTargetSystemId(); - ~MavLinkConnectionImpl(); - void startListening(std::shared_ptr parent, const std::string& nodeName, std::shared_ptr connectedPort); - void startLoggingSendMessage(std::shared_ptr log); - void stopLoggingSendMessage(); - void close(); + std::string getName(); + int getTargetComponentId(); + int getTargetSystemId(); + ~MavLinkConnectionImpl(); + void startListening(std::shared_ptr parent, const std::string& nodeName, std::shared_ptr connectedPort); + void startLoggingSendMessage(std::shared_ptr log); + void stopLoggingSendMessage(); + void close(); bool isOpen(); - void sendMessage(const MavLinkMessageBase& msg); - void sendMessage(const MavLinkMessage& msg); - int subscribe(MessageHandler handler); - void unsubscribe(int id); - uint8_t getNextSequence(); - void join(std::shared_ptr remote, bool subscribeToLeft = true, bool subscribeToRight = true); - void getTelemetry(MavLinkTelemetry& result); + void sendMessage(const MavLinkMessageBase& msg); + void sendMessage(const MavLinkMessage& msg); + int subscribe(MessageHandler handler); + void unsubscribe(int id); + uint8_t getNextSequence(); + void join(std::shared_ptr remote, bool subscribeToLeft = true, bool subscribeToRight = true); + void getTelemetry(MavLinkTelemetry& result); void ignoreMessage(uint8_t message_id); int prepareForSending(MavLinkMessage& msg); - private: - static std::shared_ptr createConnection(const std::string& nodeName, std::shared_ptr port); + private: + static std::shared_ptr createConnection(const std::string& nodeName, std::shared_ptr port); void joinLeftSubscriber(std::shared_ptr remote, std::shared_ptrcon, const MavLinkMessage& msg); void joinRightSubscriber(std::shared_ptrcon, const MavLinkMessage& msg); - void publishPackets(); - void readPackets(); - void drainQueue(); - std::string name; - std::shared_ptr port; - std::shared_ptr con_; - int other_system_id = -1; - int other_component_id = 0; - uint8_t next_seq = 0; - std::thread read_thread; - std::string accept_node_name_; - std::shared_ptr server_; - std::shared_ptr sendLog_; + void publishPackets(); + void readPackets(); + void drainQueue(); + std::string name; + std::shared_ptr port; + std::shared_ptr con_; + int other_system_id = -1; + int other_component_id = 0; + uint8_t next_seq = 0; + std::thread read_thread; + std::string accept_node_name_; + std::shared_ptr server_; + std::shared_ptr sendLog_; - struct MessageHandlerEntry { - public: - int id; - MessageHandler handler; - }; - std::vector listeners; - std::vector snapshot; - bool snapshot_stale; - std::mutex listener_mutex; - uint8_t message_buf[300]; // must be bigger than sizeof(mavlink_message_t), which is currently 292. - std::mutex buffer_mutex; - bool closed; - std::thread publish_thread_; - std::queue msg_queue_; - std::mutex msg_queue_mutex_; - mavlink_utils::Semaphore msg_available_; - bool waiting_for_msg_ = false; + struct MessageHandlerEntry { + public: + int id; + MessageHandler handler; + }; + std::vector listeners; + std::vector snapshot; + bool snapshot_stale; + std::mutex listener_mutex; + uint8_t message_buf[300]; // must be bigger than sizeof(mavlink_message_t), which is currently 292. + std::mutex buffer_mutex; + bool closed; + std::thread publish_thread_; + std::queue msg_queue_; + std::mutex msg_queue_mutex_; + mavlink_utils::Semaphore msg_available_; + bool waiting_for_msg_ = false; bool supports_mavlink2_ = false; bool signing_ = false; mavlink_status_t mavlink_intermediate_status_; mavlink_status_t mavlink_status_; std::mutex telemetry_mutex_; - MavLinkTelemetry telemetry_; + MavLinkTelemetry telemetry_; std::unordered_set ignored_messageids; - }; + }; } #endif diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 00b76f84..4b73733a 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -453,6 +453,9 @@ class AirSimClientBase: def getHomeGeoPoint(self): return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint')) + def armDisarm(self, arm): + return self.client.call('armDisarm', arm) + # basic flight control def enableApiControl(self, is_enabled): return self.client.call('enableApiControl', is_enabled) @@ -510,9 +513,6 @@ class MultirotorClient(AirSimClientBase, object): ip = "127.0.0.1" super(MultirotorClient, self).__init__(ip, 41451) - def armDisarm(self, arm): - return self.client.call('armDisarm', arm) - def takeoff(self, max_wait_seconds = 15): return self.client.call('takeoff', max_wait_seconds) diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index f659f7df..defc1d50 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -35,6 +35,9 @@ Code + + + Code @@ -63,6 +66,10 @@ Code + + + Code + Code diff --git a/PythonClient/reset_test_car.py b/PythonClient/reset_test_car.py new file mode 100644 index 00000000..d2d1c30b --- /dev/null +++ b/PythonClient/reset_test_car.py @@ -0,0 +1,25 @@ +from AirSimClient import * + +# connect to the AirSim simulator +client = CarClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) +car_controls = CarControls() + +# go forward +car_controls.throttle = 1 +car_controls.steering = 1 +client.setCarControls(car_controls) +print("Go Foward") +time.sleep(5) # let car drive a bit + +print("reset") +client.reset() +time.sleep(5) # let car drive a bit + +client.setCarControls(car_controls) +print("Go Foward") +time.sleep(5) # let car drive a bit + + diff --git a/PythonClient/reset_test_drone.py b/PythonClient/reset_test_drone.py new file mode 100644 index 00000000..b1358b99 --- /dev/null +++ b/PythonClient/reset_test_drone.py @@ -0,0 +1,23 @@ +from AirSimClient import * + + +# connect to the AirSim simulator +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +print("fly") +client.moveToPosition(0, 0, -10, 5) +time.sleep(5) # let car drive a bit + +print("reset") +client.reset() +client.enableApiControl(True) +client.armDisarm(True) +time.sleep(5) # let car drive a bit + + +print("fly") +client.moveToPosition(0, 0, -10, 5) +time.sleep(5) # let car drive a bit \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index ecc13e87..209ae7fc 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -214,15 +214,6 @@ void ACarPawn::initializeForBeginPlay(bool engine_sound) } -void ACarPawn::reset(bool disable_api_control) -{ - keyboard_controls_ = joystick_controls_ = CarPawnApi::CarControls(); - wrapper_->setApi(std::unique_ptr()); - - if (disable_api_control) - getApi()->enableApiControl(false); -} - msr::airlib::CarApiBase* ACarPawn::getApi() const { return static_cast(wrapper_->getApi()); @@ -378,10 +369,11 @@ void ACarPawn::updateCarControls() if (wrapper_->getRemoteControlID() >= 0 && joystick_state_.is_initialized) { joystick_.getJoyStickState(0, joystick_state_); - if ((joystick_state_.buttons & 4) | (joystick_state_.buttons & 1024)) { //X button or Start button - reset(); - return; - } + //TODO: move this to SimModeBase + //if ((joystick_state_.buttons & 4) | (joystick_state_.buttons & 1024)) { //X button or Start button + // reset(); + // return; + //} std::string vendorid = joystick_state_.pid_vid.substr(0, joystick_state_.pid_vid.find('&')); diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index fb10636a..faa6428d 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -81,8 +81,6 @@ public: void setupInputBindings(); - void reset(bool disable_api_control = true); - // Begin Actor interface virtual void Tick(float Delta) override; virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index 9c826621..e104d6ca 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -1,7 +1,6 @@ #include "CarPawnApi.h" #include "AirBlueprintLib.h" - CarPawnApi::CarPawnApi(VehiclePawnWrapper* pawn, UWheeledVehicleMovementComponent* movement) : pawn_(pawn), movement_(movement) { @@ -43,6 +42,13 @@ msr::airlib::CollisionInfo CarPawnApi::getCollisionInfo() const return pawn_->getCollisionInfo(); } +bool CarPawnApi::armDisarm(bool arm) +{ + //TODO: implement arming for car + unused(arm); + return true; +} + std::vector CarPawnApi::simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const { std::vector request = { ImageCaptureBase::ImageRequest(camera_id, image_type) }; @@ -118,6 +124,7 @@ void CarPawnApi::reset() auto phys_comps = UAirBlueprintLib::getPhysicsComponents(pawn_->getPawn()); UAirBlueprintLib::RunCommandOnGameThread([this, &phys_comps]() { pawn_->reset(); + for (auto* phys_comp : phys_comps) { phys_comp->SetPhysicsAngularVelocity(FVector::ZeroVector); phys_comp->SetPhysicsLinearVelocity(FVector::ZeroVector); diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h index db5a4063..54f08d94 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h @@ -42,6 +42,7 @@ public: virtual void enableApiControl(bool is_enabled) override; virtual bool isApiControlEnabled() const override; + virtual bool armDisarm(bool arm) override; virtual const CarApiBase::CarControls& getCarControls() const override; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index c25528c3..1f9e0608 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -2,6 +2,7 @@ #include "ConstructorHelpers.h" #include "AirBlueprintLib.h" #include "common/AirSimSettings.hpp" +#include "AirBlueprintLib.h" #include "Car/CarPawn.h" #ifndef AIRLIB_NO_RPC @@ -201,18 +202,11 @@ void ASimModeCar::createVehicles(std::vector& vehicles) void ASimModeCar::reset() { - //find all vehicle pawns - { - TArray pawns; - UAirBlueprintLib::FindAllActor(this, pawns); - - //set up vehicle pawns - for (AActor* pawn : pawns) - { - //initialize each vehicle pawn we found - TVehiclePawn* vehicle_pawn = static_cast(pawn); - vehicle_pawn->reset(); - } + msr::airlib::VehicleApiBase* api = getVehicleApi(); + if (api) { + UAirBlueprintLib::RunCommandOnGameThread([api]() { + api->reset(); + }, true); } Super::reset(); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp index 59b58b7b..4b644c07 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp @@ -319,23 +319,14 @@ msr::airlib::MultirotorApi* MultiRotorConnector::getApi() const //*** Start: UpdatableState implementation ***// void MultiRotorConnector::reset() { - if (UAirBlueprintLib::IsInGameThread()) - resetPrivate(); - else { - //schedule the task which we will execute in tick event when World object is locked - reset_task_ = std::packaged_task([this]() { resetPrivate(); }); - std::future reset_result = reset_task_.get_future(); - reset_pending_ = true; - did_reset_ = false; - reset_result.wait(); - } + resetPrivate(); } void MultiRotorConnector::resetPrivate() { VehicleConnectorBase::reset(); - //TODO: should this be done in MultiRotor.hpp + //TODO: should this be done in MultiRotor.hpp? //controller_->reset(); rc_data_ = RCData(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index cde38fa5..349753c1 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -71,7 +71,6 @@ msr::airlib::SimModeApiBase* ASimModeBase::getSimModeApi() const return simmode_api_.get(); } - void ASimModeBase::setupTimeOfDay() { sky_sphere_ = nullptr; @@ -308,6 +307,7 @@ void ASimModeBase::stopRecording() FRecordingThread::stopRecording(); } + //************************* SimModeApi *****************************/ ASimModeBase::SimModeApi::SimModeApi(ASimModeBase* simmode) @@ -315,6 +315,11 @@ ASimModeBase::SimModeApi::SimModeApi(ASimModeBase* simmode) { } +void ASimModeBase::SimModeApi::reset() +{ + simmode_->reset(); +} + msr::airlib::VehicleApiBase* ASimModeBase::SimModeApi::getVehicleApi() { return simmode_->getVehicleApi(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 123ddca5..839a06ae 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -82,6 +82,7 @@ private: public: SimModeApi(ASimModeBase* simmode); virtual msr::airlib::VehicleApiBase* getVehicleApi() override; + virtual void reset() override; virtual bool isPaused() const override; virtual void pause(bool is_paused) override; virtual void continueForTime(double seconds) override; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 1115cb96..7929fcfa 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -1,5 +1,6 @@ #include "SimModeWorldBase.h" #include +#include "AirBlueprintLib.h" void ASimModeWorldBase::BeginPlay() @@ -131,7 +132,9 @@ void ASimModeWorldBase::Tick(float DeltaSeconds) void ASimModeWorldBase::reset() { - physics_world_->reset(); + UAirBlueprintLib::RunCommandOnGameThread([this]() { + physics_world_->reset(); + }, true); Super::reset(); } diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index 51dc873a..e3d72569 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp @@ -217,12 +217,15 @@ int VehiclePawnWrapper::getCameraCount() void VehiclePawnWrapper::reset() { state_ = initial_state_; + pawn_->SetActorLocationAndRotation(state_.start_location, state_.start_rotation, false, nullptr, ETeleportType::TeleportPhysics); +} + +//void playBack() +//{ //if (pawn_->GetRootPrimitiveComponent()->IsAnySimulatingPhysics()) { // pawn_->GetRootPrimitiveComponent()->SetSimulatePhysics(false); // pawn_->GetRootPrimitiveComponent()->SetSimulatePhysics(true); //} - pawn_->SetActorLocationAndRotation(state_.start_location, state_.start_rotation, false, nullptr, ETeleportType::TeleportPhysics); - //TODO: refactor below code used for playback //std::ifstream sim_log("C:\\temp\\mavlogs\\circle\\sim_cmd_006_orbit 5 1.txt.pos.txt"); //plot(sim_log, FColor::Purple, Vector3r(0, 0, -3)); @@ -233,7 +236,7 @@ void VehiclePawnWrapper::reset() //plot(sim_log, FColor::Purple, Vector3r(0, 0, -3)); //std::ifstream real_log("C:\\temp\\mavlogs\\square\\real_cmd_012_square 5 1.txt.pos.txt"); //plot(real_log, FColor::Yellow, Vector3r(0, 0, -3)); -} +//} const VehiclePawnWrapper::GeoPoint& VehiclePawnWrapper::getHomePoint() const { diff --git a/docs/apis.md b/docs/apis.md index d7e0c3ce..aace2a91 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -85,7 +85,7 @@ You can find a ready to run project in HelloCar folder in the repository. ## Common APIs -* `reset`: This resets the vehicle to its original starting state. +* `reset`: This resets the vehicle to its original starting state. Note that you must call `enableApiControl` and `armDisarm` again after the call to `reset`. * `confirmConnection`: Checks state of connection every 1 sec and reports it in Console so user can see the progress for connection. * `enableApiControl`: For safety reasons, by default API control for autonomous vehicle is not enabled and human operator has full control (usually via RC or joystick in simulator). The client must make this call to request control via API. It is likely that human operator of vehicle might have disallowed API control which would mean that enableApiControl has no effect. This can be checked by `isApiControlEnabled`. * `isApiControlEnabled`: Returns true if API control is established. If false (which is default) then API calls would be ignored. After a successful call to `enableApiControl`, the `isApiControlEnabled` should return true. diff --git a/docs/reinforcement_learning.md b/docs/reinforcement_learning.md index 6a3c4f07..9ee1bd49 100644 --- a/docs/reinforcement_learning.md +++ b/docs/reinforcement_learning.md @@ -89,6 +89,8 @@ If the episode terminates then we reset the vehicle to the original state via: ``` client.reset() +client.enableApiControl(True) +client.armDisarm(True) car_control = interpret_action(1) // Reset position and drive straight for one second client.setCarControls(car_control) time.sleep(1)