Easier integration in gym RL environment

.pre-commit-config.yaml

@@ -0,0 +1,26 @@
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v5.0.0
+    hooks:
+      - id: trailing-whitespace
+      - id: end-of-file-fixer
+      - id: check-toml
+      - id: check-yaml
+      - id: check-added-large-files
+      - id: check-case-conflict
+      - id: check-merge-conflict
+      - id: detect-private-key
+
+  - repo: https://github.com/astral-sh/ruff-pre-commit
+    rev: v0.14.11
+    hooks:
+      - id: ruff-check
+        args: [--fix, --exit-non-zero-on-fix]
+      - id: ruff-format
+
+  - repo: https://github.com/pre-commit/mirrors-mypy
+    rev: v1.19.1
+    hooks:
+      - id: mypy
+        additional_dependencies: []
+        args: [--ignore-missing-imports, --strict]

AirSim/AirLib/include/api/RpcLibClientBase.hpp

@@ -63,7 +63,7 @@ class RpcLibClientBase {
     void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
     void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
     void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent);
-    void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration); 
+    void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration);
     void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent);
     void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration);
 
@@ -84,6 +84,8 @@ class RpcLibClientBase {
 
     Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
     void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");
+    void simResetToPose(const Pose& pose, const std::string& vehicle_name = "");
+    void simSetVehicleLinearVelocity(const Vector3r& velocity, const std::string& vehicle_name = "");
     void simSetTraceLine(const std::vector<float>& color_rgba, float thickness=3.0f, const std::string& vehicle_name = "");
 
     vector<ImageCaptureBase::ImageResponse> simGetImages(vector<ImageCaptureBase::ImageRequest> request, const std::string& vehicle_name = "");

AirSim/AirLib/include/api/VehicleSimApiBase.hpp

@@ -23,7 +23,7 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     }
 
     //this method is called at every render tick when we want to transfer state from
-    //physics engine to render engine. As physics engine is halted while 
+    //physics engine to render engine. As physics engine is halted while
     //this happens, this method should do minimal processing
     virtual void updateRenderedState(float dt)
     {
@@ -50,6 +50,8 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
 
     virtual Pose getPose() const = 0;
     virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
+    virtual void resetToPose(const Pose& pose) = 0;
+    virtual void setLinearVelocity(const Vector3r& velocity) = 0;
     virtual const Kinematics::State* getGroundTruthKinematics() const = 0;
     virtual const WheelStates* getWheelStates() const = 0;
 

AirSim/AirLib/include/api/WorldSimApiBase.hpp

@@ -28,6 +28,7 @@ class WorldSimApiBase {
 
     virtual bool isPaused() const = 0;
     virtual void reset() = 0;
+    virtual void resetToPose(const Pose& pose, const std::string& vehicle_name = "") = 0;
     virtual void pause(bool is_paused) = 0;
     virtual void continueForTime(double seconds) = 0;
 
@@ -45,12 +46,12 @@ class WorldSimApiBase {
 
     //----------- Plotting APIs ----------/
     virtual void simFlushPersistentMarkers() = 0;
-    virtual void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) = 0; 
-    virtual void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0; 
-    virtual void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0; 
-    virtual void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0; 
+    virtual void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) = 0;
+    virtual void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
+    virtual void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
+    virtual void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0;
     virtual void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration) = 0;
-    virtual void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) = 0; 
+    virtual void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) = 0;
     virtual void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration) = 0;
 
     virtual std::vector<std::string> listSceneObjects(const std::string& name_regex) const = 0;

AirSim/AirLib/src/api/RpcLibClientBase.cpp

@@ -40,7 +40,7 @@ STRICT_MODE_OFF
 STRICT_MODE_ON
 #ifdef _MSC_VER
 __pragma(warning( disable : 4239))
-#endif			  
+#endif
 
 
 namespace msr { namespace airlib {
@@ -125,7 +125,7 @@ void RpcLibClientBase::confirmConnection(double timeout)
         clock->sleep_for(delay);
     }
 
-    
+
     if (getConnectionState() != RpcLibClientBase::ConnectionState::Connected)
     {
         throw std::runtime_error("Failed connecting to RPC server (airsim). Is the simulator running?");
@@ -135,7 +135,7 @@ void RpcLibClientBase::confirmConnection(double timeout)
     auto client_ver = getClientVersion();
     auto server_min_ver = getMinRequiredServerVersion();
     auto client_min_ver = getMinRequiredClientVersion();
-    
+
     std::string ver_info = Utils::stringf("Client Ver:%i (Min Req:%i), Server Ver:%i (Min Req:%i)",
         client_ver, client_min_ver, server_ver, server_min_ver);
 
@@ -208,14 +208,24 @@ void RpcLibClientBase::simSetVehiclePose(const Pose& pose, bool ignore_collision
     pimpl_->client.call("simSetVehiclePose", RpcLibAdapatorsBase::Pose(pose), ignore_collision, vehicle_name);
 }
 
+void RpcLibClientBase::simResetToPose(const Pose& pose, const std::string& vehicle_name)
+{
+    pimpl_->client.call("simResetToPose", RpcLibAdapatorsBase::Pose(pose), vehicle_name);
+}
+
+void RpcLibClientBase::simSetVehicleLinearVelocity(const Vector3r& velocity, const std::string& vehicle_name)
+{
+    pimpl_->client.call("simSetVehicleLinearVelocity", RpcLibAdapatorsBase::Vector3r(velocity), vehicle_name);
+}
+
 void RpcLibClientBase::simSetTraceLine(const std::vector<float>& color_rgba, float thickness, const std::string & vehicle_name)
 {
     pimpl_->client.call("simSetTraceLine", color_rgba, thickness, vehicle_name);
 }
 
 vector<ImageCaptureBase::ImageResponse> RpcLibClientBase::simGetImages(vector<ImageCaptureBase::ImageRequest> request, const std::string& vehicle_name)
 {
-    const auto& response_adaptor = pimpl_->client.call("simGetImages", 
+    const auto& response_adaptor = pimpl_->client.call("simGetImages",
         RpcLibAdapatorsBase::ImageRequest::from(request), vehicle_name)
         .as<vector<RpcLibAdapatorsBase::ImageResponse>>();
 

AirSim/AirLib/src/api/RpcLibServerBase.cpp

@@ -49,7 +49,7 @@ struct RpcLibServerBase::impl {
     ~impl() {
     }
 
-    void stop() {        
+    void stop() {
         server.close_sessions();
         if (!is_async_) {
             // this deadlocks UI thread if async_run was called while there are pending rpc calls.
@@ -92,20 +92,20 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
      pimpl_->server.bind("getSettingsString", [&]() -> std::string {
         return getWorldSimApi()->getSettingsString();
     });
-       
-    pimpl_->server.bind("simPause", [&](bool is_paused) -> void { 
-        getWorldSimApi()->pause(is_paused); 
+
+    pimpl_->server.bind("simPause", [&](bool is_paused) -> void {
+        getWorldSimApi()->pause(is_paused);
     });
-    pimpl_->server.bind("simIsPaused", [&]() -> bool { 
-        return getWorldSimApi()->isPaused(); 
+    pimpl_->server.bind("simIsPaused", [&]() -> bool {
+        return getWorldSimApi()->isPaused();
     });
-    pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { 
-        getWorldSimApi()->continueForTime(seconds); 
+    pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void {
+        getWorldSimApi()->continueForTime(seconds);
     });
 
-    pimpl_->server.bind("simSetTimeOfDay", [&](bool is_enabled, const string& start_datetime, bool is_start_datetime_dst, 
+    pimpl_->server.bind("simSetTimeOfDay", [&](bool is_enabled, const string& start_datetime, bool is_start_datetime_dst,
         float celestial_clock_speed, float update_interval_secs, bool move_sun) -> void {
-        getWorldSimApi()->setTimeOfDay(is_enabled, start_datetime, is_start_datetime_dst, 
+        getWorldSimApi()->setTimeOfDay(is_enabled, start_datetime, is_start_datetime_dst,
             celestial_clock_speed, update_interval_secs, move_sun);
     });
 
@@ -116,13 +116,13 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         getWorldSimApi()->setWeatherParameter(param, val);
     });
 
-    pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { 
+    pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void {
         getVehicleApi(vehicle_name)->enableApiControl(is_enabled);
     });
-    pimpl_->server.bind("isApiControlEnabled", [&](const std::string& vehicle_name) -> bool { 
+    pimpl_->server.bind("isApiControlEnabled", [&](const std::string& vehicle_name) -> bool {
         return getVehicleApi(vehicle_name)->isApiControlEnabled();
     });
-    pimpl_->server.bind("simGetImages", [&](const std::vector<RpcLibAdapatorsBase::ImageRequest>& request_adapter, const std::string& vehicle_name) -> 
+    pimpl_->server.bind("simGetImages", [&](const std::vector<RpcLibAdapatorsBase::ImageRequest>& request_adapter, const std::string& vehicle_name) ->
         vector<RpcLibAdapatorsBase::ImageResponse> {
             const auto& response = getVehicleSimApi(vehicle_name)->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter));
             return RpcLibAdapatorsBase::ImageResponse::from(response);
@@ -150,6 +150,10 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::Pose(pose);
     });
 
+    pimpl_->server.bind("simSetVehicleLinearVelocity", [&](const RpcLibAdapatorsBase::Vector3r& velocity, const std::string& vehicle_name) -> void {
+        getVehicleSimApi(vehicle_name)->setLinearVelocity(velocity.to());
+    });
+
     pimpl_->server.bind("simSetTraceLine", [&](const std::vector<float>& color_rgba, float thickness, const std::string& vehicle_name) -> void {
         getVehicleSimApi(vehicle_name)->setTraceLine(color_rgba, thickness);
     });
@@ -166,7 +170,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
     pimpl_->server.
         bind("simGetSegmentationObjectID", [&](const std::string& mesh_name) -> int {
         return getWorldSimApi()->getSegmentationObjectID(mesh_name);
-    });    
+    });
 
     pimpl_->server.bind("reset", [&]() -> void {
         //Exit if already resetting.
@@ -184,6 +188,10 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         resetInProgress = false;
     });
 
+    pimpl_->server.bind("simResetToPose", [&](const RpcLibAdapatorsBase::Pose& pose, const std::string& vehicle_name) -> void {
+        getWorldSimApi()->resetToPose(pose.to(), vehicle_name);
+    });
+
     pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void {
         getWorldSimApi()->printLogMessage(message, message_param, severity);
     });
@@ -223,7 +231,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::CameraInfo(camera_info);
     });
 
-    pimpl_->server.bind("simSetCameraOrientation", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Quaternionr& orientation, 
+    pimpl_->server.bind("simSetCameraOrientation", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Quaternionr& orientation,
         const std::string& vehicle_name) -> void {
         getVehicleSimApi(vehicle_name)->setCameraOrientation(camera_name, orientation.to());
     });
@@ -234,7 +242,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
     });
 
     pimpl_->server.bind("simGetCollisionInfo", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::CollisionInfo {
-        const auto& collision_info = getVehicleSimApi(vehicle_name)->getCollisionInfo(); 
+        const auto& collision_info = getVehicleSimApi(vehicle_name)->getCollisionInfo();
         return RpcLibAdapatorsBase::CollisionInfo(collision_info);
     });
 
@@ -243,7 +251,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
     });
 
     pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose {
-        const auto& pose = getWorldSimApi()->getObjectPose(object_name); 
+        const auto& pose = getWorldSimApi()->getObjectPose(object_name);
         return RpcLibAdapatorsBase::Pose(pose);
     });
     pimpl_->server.bind("simSetObjectPose", [&](const std::string& object_name, const RpcLibAdapatorsBase::Pose& pose, bool teleport) -> bool {

UE4Project/Plugins/AirSim/Source/CoordFrameTransformer.cpp

@@ -34,6 +34,10 @@ CoordFrameTransformer::Vector3r CoordFrameTransformer::toLocalEnuVelocity(const
 {
     return CoordFrameTransformer::toVector3r(velocity, 1 / world_to_meters_);
 }
+FVector CoordFrameTransformer::fromLocalEnuVelocity(const Vector3r& velocity) const
+{
+    return CoordFrameTransformer::toFVector(velocity, world_to_meters_);
+}
 CoordFrameTransformer::Vector3r CoordFrameTransformer::toGlobalEnu(const FVector& position) const
 {
     return CoordFrameTransformer::toVector3r(position - global_transform_.GetLocation(), 1 / world_to_meters_);

UE4Project/Plugins/AirSim/Source/CoordFrameTransformer.h

@@ -29,6 +29,7 @@ class AIRSIM_API CoordFrameTransformer
     //UU -> local ENU
     Vector3r toLocalEnu(const FVector& position) const;
     Vector3r toLocalEnuVelocity(const FVector& velocity) const;
+    FVector fromLocalEnuVelocity(const Vector3r& velocity) const;
     Vector3r toGlobalEnu(const FVector& position) const;
     Quaternionr toEnu(const FQuat& q) const;
     float toEnu(float length) const;