Merge pull request #2937 from rajat2004/fix-unity-car-api

[Unity] Fix Car API

Unity/AirLibWrapper/AirsimWrapper/Source/UnityImageCapture.cpp

@@ -4,6 +4,7 @@
 
 namespace AirSimUnity
 {
+
 UnityImageCapture::UnityImageCapture(std::string vehicle_name)
     : vehicle_name_(vehicle_name)
 {
@@ -17,14 +18,13 @@ UnityImageCapture::~UnityImageCapture()
 void UnityImageCapture::getImages(const std::vector<msr::airlib::ImageCaptureBase::ImageRequest>& requests,
                                   std::vector<msr::airlib::ImageCaptureBase::ImageResponse>& responses) const
 {
-    if (requests.size() > 0) {
-        for (int i = 0; i < requests.size(); i++) {
-            ImageResponse airsim_response;
-            responses.push_back(airsim_response);
-            AirSimImageRequest request = UnityUtilities::Convert_to_UnityRequest(requests[i]);
-            AirSimImageResponse response = GetSimImages(request, vehicle_name_.c_str()); //Into Unity
-            UnityUtilities::Convert_to_AirsimResponse(response, responses[i], request.camera_name);
-        }
+    for (auto i = 0u; i < requests.size(); i++) {
+        ImageResponse airsim_response;
+        responses.push_back(airsim_response);
+        AirSimImageRequest request = UnityUtilities::Convert_to_UnityRequest(requests[i]);
+        AirSimImageResponse response = GetSimImages(request, vehicle_name_.c_str()); //Into Unity
+        UnityUtilities::Convert_to_AirsimResponse(response, responses[i], request.camera_name);
     }
 }
-}
+
+}

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.cpp

@@ -1,9 +1,9 @@
 #include "CarPawnApi.h"
 #include "../../PInvokeWrapper.h"
 
-CarPawnApi::CarPawnApi(CarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics,
+CarPawnApi::CarPawnApi(const msr::airlib::Kinematics::State* pawn_kinematics,
                        const std::string car_name, msr::airlib::CarApiBase* vehicle_api)
-    : pawn_(pawn), pawn_kinematics_(pawn_kinematics), car_name_(car_name), vehicle_api_(vehicle_api)
+    : pawn_kinematics_(pawn_kinematics), car_name_(car_name), vehicle_api_(vehicle_api)
 {
 }
 
@@ -13,6 +13,11 @@ void CarPawnApi::updateMovement(const msr::airlib::CarApiBase::CarControls& cont
     SetCarApiControls(controls, car_name_.c_str());
 }
 
+void CarPawnApi::enableApi(bool enable)
+{
+    SetEnableApi(enable, car_name_.c_str());
+}
+
 msr::airlib::CarApiBase::CarState CarPawnApi::getCarState() const
 {
     AirSimCarState carState = GetCarState(car_name_.c_str());

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.h

@@ -2,19 +2,19 @@
 
 #include "vehicles/car/api/CarApiBase.hpp"
 #include "physics/Kinematics.hpp"
-#include "CarPawn.h"
 
 class CarPawnApi
 {
 public:
     typedef msr::airlib::ImageCaptureBase ImageCaptureBase;
 
 public:
-    CarPawnApi(CarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics,
+    CarPawnApi(const msr::airlib::Kinematics::State* pawn_kinematics,
                const std::string car_name, msr::airlib::CarApiBase* vehicle_api);
 
     void updateMovement(const msr::airlib::CarApiBase::CarControls& controls);
     msr::airlib::CarApiBase::CarState getCarState() const;
+    void enableApi(bool enable);
 
     void reset();
     void update();
@@ -23,7 +23,6 @@ class CarPawnApi
 
 private:
     msr::airlib::CarApiBase::CarControls last_controls_;
-    CarPawn* pawn_;
     const msr::airlib::Kinematics::State* pawn_kinematics_;
     std::string car_name_;
     msr::airlib::CarApiBase* vehicle_api_;

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp

@@ -6,30 +6,27 @@
 #include "../../UnityUtilities.hpp"
 #include "../../UnitySensors/UnitySensorFactory.h"
 
-CarPawnSimApi::CarPawnSimApi(const Params& params,
-                             const msr::airlib::CarApiBase::CarControls& keyboard_controls, std::string car_name)
-    : PawnSimApi(params), params_(params), keyboard_controls_(keyboard_controls), car_name_(car_name)
+CarPawnSimApi::CarPawnSimApi(const Params& params, std::string car_name)
+    : PawnSimApi(params), params_(params), car_name_(car_name)
 {
-    createVehicleApi(static_cast<CarPawn*>(params.pawn), params.home_geopoint);
-    joystick_controls_ = msr::airlib::CarApiBase::CarControls();
 }
 
 void CarPawnSimApi::initialize()
 {
     PawnSimApi::initialize();
 
-    createVehicleApi(static_cast<CarPawn*>(params_.pawn), params_.home_geopoint);
+    createVehicleApi(params_.home_geopoint);
 
     //TODO: should do reset() here?
     joystick_controls_ = msr::airlib::CarApiBase::CarControls();
 }
 
-void CarPawnSimApi::createVehicleApi(CarPawn* pawn, const msr::airlib::GeoPoint& home_geopoint)
+void CarPawnSimApi::createVehicleApi(const msr::airlib::GeoPoint& home_geopoint)
 {
     std::shared_ptr<UnitySensorFactory> sensor_factory = std::make_shared<UnitySensorFactory>(car_name_, &getNedTransform());
 
     vehicle_api_ = CarApiFactory::createApi(getVehicleSetting(), sensor_factory, (*getGroundTruthKinematics()), (*getGroundTruthEnvironment()), home_geopoint);
-    pawn_api_ = std::unique_ptr<CarPawnApi>(new CarPawnApi(pawn, getGroundTruthKinematics(), car_name_, vehicle_api_.get()));
+    pawn_api_ = std::unique_ptr<CarPawnApi>(new CarPawnApi(getGroundTruthKinematics(), car_name_, vehicle_api_.get()));
 }
 
 std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const
@@ -79,8 +76,8 @@ void CarPawnSimApi::updateRendering(float dt)
     PawnSimApi::updateRendering(dt);
     updateCarControls();
 
-    for (auto i = 0; i < vehicle_api_messages_.size(); ++i) {
-        PrintLogMessage(vehicle_api_messages_[i].c_str(), "LogDebugLevel::Success", car_name_.c_str(), ErrorLogSeverity::Information);
+    for (const auto& message : vehicle_api_messages_) {
+        PrintLogMessage(message.c_str(), "LogDebugLevel::Success", car_name_.c_str(), ErrorLogSeverity::Information);
     }
 
     try {
@@ -137,20 +134,24 @@ void CarPawnSimApi::updateCarControls()
     }
     else {
         PrintLogMessage("Control Mode: ", "Keyboard", getVehicleName().c_str(), ErrorLogSeverity::Information);
-        current_controls_ = keyboard_controls_;
     }
 
+    bool api_enabled = vehicle_api_->isApiControlEnabled();
+
     //if API-client control is not active then we route keyboard/joystick control to car
-    if (!vehicle_api_->isApiControlEnabled()) {
-        //all car controls from anywhere must be routed through API component
+    if (!api_enabled) {
+        // This is so that getCarControls API works correctly
         vehicle_api_->setCarControls(current_controls_);
-        pawn_api_->updateMovement(current_controls_);
     }
     else {
         PrintLogMessage("Control Mode: ", "API", getVehicleName().c_str(), ErrorLogSeverity::Information);
+        // API is enabled, so we use the controls set by API
         current_controls_ = vehicle_api_->getCarControls();
-        pawn_api_->updateMovement(current_controls_);
     }
+
+    // Update whether to use API controls or keyboard controls
+    pawn_api_->enableApi(api_enabled);
+    pawn_api_->updateMovement(current_controls_);
 }
 
 //*** Start: UpdatableState implementation ***//

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include "CarPawn.h"
 #include "CarPawnApi.h"
 #include "../../PawnSimApi.h"
 #include "vehicles/car/api/CarApiBase.hpp"
@@ -15,12 +14,12 @@ class CarPawnSimApi : public PawnSimApi
     typedef msr::airlib::Pose Pose;
 
 private:
-    void createVehicleApi(CarPawn* pawn, const msr::airlib::GeoPoint& home_geopoint);
+    void createVehicleApi(const msr::airlib::GeoPoint& home_geopoint);
     void updateCarControls();
 
 public:
     virtual void initialize() override;
-    CarPawnSimApi(const Params& params, const msr::airlib::CarApiBase::CarControls& keyboard_controls, std::string car_name);
+    CarPawnSimApi(const Params& params, std::string car_name);
     virtual ~CarPawnSimApi() = default;
 
     virtual void update() override;
@@ -46,7 +45,4 @@ class CarPawnSimApi : public PawnSimApi
     msr::airlib::CarApiBase::CarControls joystick_controls_;
     msr::airlib::CarApiBase::CarControls current_controls_;
     std::string car_name_;
-
-    //storing reference from pawn
-    const msr::airlib::CarApiBase::CarControls& keyboard_controls_;
 };

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/SimModeCar.cpp

@@ -88,11 +88,7 @@ UnityPawn* SimModeCar::GetVehiclePawn()
 
 std::unique_ptr<PawnSimApi> SimModeCar::createVehicleSimApi(const PawnSimApi::Params& pawn_sim_api_params) const
 {
-    auto vehicle_pawn = static_cast<TVehiclePawn*>(pawn_sim_api_params.pawn);
-
-    auto vehicle_sim_api = std::unique_ptr<PawnSimApi>(new CarPawnSimApi(pawn_sim_api_params,
-                                                                         vehicle_pawn->getKeyBoardControls(),
-                                                                         vehicle_name_));
+    auto vehicle_sim_api = std::unique_ptr<PawnSimApi>(new CarPawnSimApi(pawn_sim_api_params, vehicle_name_));
     vehicle_sim_api->initialize();
     vehicle_sim_api->reset();
     return vehicle_sim_api;
@@ -102,4 +98,4 @@ msr::airlib::VehicleApiBase* SimModeCar::getVehicleApi(const PawnSimApi::Params&
 {
     const auto car_sim_api = static_cast<const CarPawnSimApi*>(sim_api);
     return car_sim_api->getVehicleApi();
-}
+}

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/FlyingPawn.cpp

@@ -10,7 +10,7 @@ FlyingPawn::FlyingPawn(std::string multirotor_name)
 
 void FlyingPawn::setRotorSpeed(const std::vector<MultirotorPawnEvents::RotorActuatorInfo>& rotor_infos)
 {
-    for (auto rotor_index = 0; rotor_index < rotor_infos.size(); ++rotor_index) {
+    for (auto rotor_index = 0u; rotor_index < rotor_infos.size(); ++rotor_index) {
         SetRotorSpeed(rotor_index, UnityUtilities::Convert_to_UnityRotorInfo(rotor_infos[rotor_index]), multirotor_name_.c_str());
     }
-}
+}

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp

@@ -102,8 +102,8 @@ void MultirotorPawnSimApi::updateRendering(float dt)
 
     PrintLogMessage("Collision Count:", std::to_string(collision_response.collision_count_non_resting).c_str(), getVehicleName().c_str(), ErrorLogSeverity::Information);
 
-    for (auto i = 0; i < vehicle_api_messages_.size(); ++i) {
-        PrintLogMessage(vehicle_api_messages_[i].c_str(), "30", getVehicleName().c_str(), ErrorLogSeverity::Information);
+    for (const auto& message : vehicle_api_messages_) {
+        PrintLogMessage(message.c_str(), "30", getVehicleName().c_str(), ErrorLogSeverity::Information);
     }
 
     try {
@@ -113,7 +113,7 @@ void MultirotorPawnSimApi::updateRendering(float dt)
         PrintLogMessage(e.what(), "LogDebugLevel::Failure, 30", getVehicleName().c_str(), ErrorLogSeverity::Error);
     }
 
-    for (int i = 0; i < rotor_actuator_info_.size(); i++) {
+    for (auto i = 0u; i < rotor_actuator_info_.size(); i++) {
         SetRotorSpeed(i, UnityUtilities::Convert_to_UnityRotorInfo(rotor_actuator_info_[i]), getVehicleName().c_str());
     }
 }

Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h

@@ -15,9 +15,9 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase
     virtual ~WorldSimApi();
 
     // ------ Level setting apis ----- //
-    virtual bool loadLevel(const std::string& level_name) { return false; };
-    virtual std::string spawnObject(std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled) { return ""; };
-    virtual bool destroyObject(const std::string& object_name) { return false; };
+    virtual bool loadLevel(const std::string& level_name) override { return false; };
+    virtual std::string spawnObject(std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled) override { return ""; };
+    virtual bool destroyObject(const std::string& object_name) override { return false; };
 
     virtual bool isPaused() const override;
     virtual void reset() override;