unify reset() mechanism with consistency across car and drone, make a…

…rmDisarm as promitive

AirLib/include/api/RpcLibClientBase.hpp

@@ -32,6 +32,7 @@ class RpcLibClientBase {
     bool isApiControlEnabled();
     void enableApiControl(bool is_enabled);
     void reset();
+    bool armDisarm(bool arm);
 
     CollisionInfo getCollisionInfo();
 

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;

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;
 

AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp

@@ -44,7 +44,7 @@ class MultirotorApi : public VehicleApiBase {
     }
     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<DirectCancelableBase>();

AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp

@@ -45,7 +45,7 @@ class DroneControllerBase : public VehicleControllerBase {
     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)
         {}
     };

AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp

@@ -44,7 +44,7 @@ class OffboardApi :
         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;

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<bool>();
+}
+
 CameraInfo RpcLibClientBase::getCameraInfo(int camera_id)
 {
     return pimpl_->client.call("getCameraInfo", camera_id).as<RpcLibAdapatorsBase::CameraInfo>().to();

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

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<rpc::client*>(getClient())->call("armDisarm", arm).as<bool>();
-}
 void MultirotorRpcLibClient::setSimulationMode(bool is_set)
 {
     static_cast<rpc::client*>(getClient())->call("setSimulationMode", is_set);

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<rpc::server*>(getServer()))->
-        bind("armDisarm", [&](bool arm) -> bool { return getDroneApi()->armDisarm(arm); });
     (static_cast<rpc::server*>(getServer()))->
         bind("setSimulationMode", [&](bool is_set) -> void { getDroneApi()->setSimulationMode(is_set); });
     (static_cast<rpc::server*>(getServer()))->

DroneServer/main.cpp

@@ -28,6 +28,11 @@ class DroneServerSimModeApi : public SimModeApiBase {
         return api_;
     }
 
+    virtual void reset() override
+    {
+        throw std::domain_error("reset is not supported");
+    }
+
     virtual bool isPaused() const override
     {
         return false;

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<MavLinkConnection>  connectSerial(const std::string& nodeName, std::string portName, int baudrate = 115200, const std::string initString = "");
+        static std::shared_ptr<MavLinkConnection>  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,21 +69,21 @@ 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<MavLinkConnection>  connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort);
+        static std::shared_ptr<MavLinkConnection>  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.
         // The localAddr can also a specific local ip address if you need to specify which
         // 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<MavLinkConnection>  connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort);
+        static std::shared_ptr<MavLinkConnection>  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<MavLinkConnection>  connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort);
+        static std::shared_ptr<MavLinkConnection>  connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort);
 
         // instance methods
         std::string getName();

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>  MavLinkConnection::connectSerial(const std::string& nodeName, std::string portName, int baudrate, const std::string initString)
+std::shared_ptr<MavLinkConnection>  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>  MavLinkConnection::connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort)
+std::shared_ptr<MavLinkConnection>  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>  MavLinkConnection::connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort)
+std::shared_ptr<MavLinkConnection>  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>  MavLinkConnection::connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort)
+std::shared_ptr<MavLinkConnection>  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<Port> 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<MavLinkLog> 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<MavLinkConnection> 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);
 }
 
 

MavLinkCom/src/impl/MavLinkConnectionImpl.cpp

@@ -45,7 +45,7 @@ std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::createConnection(cons
     return con;
 }
 
-std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort)
+std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort)
 {
     std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();
 
@@ -54,7 +54,7 @@ std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectLocalUdp(const
     return createConnection(nodeName, socket);
 }
 
-std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort)
+std::shared_ptr<MavLinkConnection>  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<MavLinkConnection>  MavLinkConnectionImpl::connectRemoteUdp(cons
     return createConnection(nodeName, socket);
 }
 
-std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort)
+std::shared_ptr<MavLinkConnection>  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<MavLinkConnection>  MavLinkConnectionImpl::connectTcp(const std:
     return createConnection(nodeName, socket);
 }
 
-std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectSerial(const std::string& nodeName, std::string name, int baudRate, const std::string initString)
+std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString)
 {
     std::shared_ptr<SerialPort> serial = std::make_shared<SerialPort>();
 
-    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) {