Implement simSetObjectPose API

AirLib/include/api/RpcLibClientBase.hpp

@@ -41,6 +41,7 @@ class RpcLibClientBase {
     void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
 
     Pose simGetObjectPose(const std::string& object_name);
+    void simSetObjectPose(const std::string& object_name, const Pose& pose);
     CameraInfo getCameraInfo(int camera_id);
     void setCameraOrientation(int camera_id, const Quaternionr& orientation);
 

AirLib/include/api/VehicleApiBase.hpp

@@ -33,6 +33,7 @@ class VehicleApiBase {
     virtual CollisionInfo getCollisionInfo() const = 0;
 
     virtual Pose simGetObjectPose(const std::string& object_name) const = 0;
+    virtual void simSetObjectPose(const std::string& object_name, const Pose& pose) = 0;
 
     virtual CameraInfo getCameraInfo(int camera_id) const = 0;
     virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;

AirLib/include/controllers/VehicleConnectorBase.hpp

@@ -29,11 +29,12 @@ class VehicleConnectorBase : public UpdatableObject
     virtual int getSegmentationObjectID(const std::string& mesh_name) = 0;
     virtual void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) = 0;
     virtual Pose getActorPose(const std::string& actor_name) = 0;
+    virtual void setActorPose(const std::string& actor_name, const Pose& pose) = 0;
     virtual Kinematics::State getTrueKinematics() = 0;
     virtual CameraInfo getCameraInfo(int camera_id) const = 0;
     virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;
 };
 
 
 }} //namespace
-#endif
+#endif

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

@@ -267,6 +267,11 @@ class MultirotorApi : public VehicleApiBase {
         return vehicle_->getActorPose(actor_name);
     }
 
+    virtual void simSetObjectPose(const std::string& actor_name, const Pose& pose) override
+    {
+        vehicle_->setActorPose(actor_name, pose);
+    }
+
     virtual void simSetPose(const Pose& pose, bool ignore_collision) override
     {
         vehicle_->setPose(pose, ignore_collision);

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

@@ -81,6 +81,12 @@ class RealMultirotorConnector : public VehicleConnectorBase
         return msr::airlib::Pose();
     }
 
+    virtual void setActorPose(const std::string& actor_name, const Pose& pose) override
+    {
+        unused(actor_name);
+        unused(pose);
+    }
+
     virtual CameraInfo getCameraInfo(int camera_id) const override
     {
         unused(camera_id);
@@ -101,4 +107,4 @@ class RealMultirotorConnector : public VehicleConnectorBase
 
 
 }} //namespace
-#endif
+#endif

AirLib/src/api/RpcLibClientBase.cpp

@@ -193,6 +193,10 @@ msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_n
     return pimpl_->client.call("simGetObjectPose", object_name).as<RpcLibAdapatorsBase::Pose>().to();
 }
 
+void RpcLibClientBase::simSetObjectPose(const std::string& object_name, const Pose& pose)
+{
+    pimpl_->client.call("simSetObjectPose", object_name, RpcLibAdapatorsBase::Pose(pose));
+}
 
 }} //namespace
 

AirLib/src/api/RpcLibServerBase.cpp

@@ -121,6 +121,10 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad
         const auto& pose = getVehicleApi()->simGetObjectPose(object_name); 
         return RpcLibAdapatorsBase::Pose(pose);
     });
+
+    pimpl_->server.bind("simSetObjectPose", [&](const std::string& object_name, const RpcLibAdapatorsBase::Pose &pose) {
+        getVehicleApi()->simSetObjectPose(object_name, pose.to());
+    });
 
     pimpl_->server.bind("simPause", [&](bool is_paused) -> void { 
         getSimModeApi()->pause(is_paused); 

PythonClient/AirSimClient.py

@@ -472,6 +472,8 @@ def simPrintLogMessage(self, message, message_param = "", severity = 0):
     def simGetObjectPose(self, object_name):
         pose = self.client.call('simGetObjectPose', object_name)
         return Pose.from_msgpack(pose)
+    def simSetObjectPose(self, object_name, pose):
+        return self.client.call('simSetObjectPose', object_name, pose)
 
 
     # camera control

Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp

@@ -86,6 +86,13 @@ msr::airlib::Pose CarPawnApi::simGetObjectPose(const std::string& actor_name) co
     return pose;
 }
 
+void CarPawnApi::simSetObjectPose(const std::string& actor_name, const msr::airlib::Pose& pose)
+{
+    UAirBlueprintLib::RunCommandOnGameThread([&pose, &actor_name, this]() {
+        pawn_->setActorPose(actor_name, pose);
+    }, true);
+}
+
 const CarApiBase::CarControls& CarPawnApi::getCarControls() const
 {
     return last_controls_;

Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h

@@ -47,6 +47,7 @@ class CarPawnApi : public msr::airlib::CarApiBase {
     virtual const CarApiBase::CarControls& getCarControls() const override;
 
     virtual msr::airlib::Pose simGetObjectPose(const std::string& actor_name) const override;
+    virtual void simSetObjectPose(const std::string& actor_name, const msr::airlib::Pose& pose) override;
     virtual msr::airlib::CameraInfo getCameraInfo(int camera_id) const override;
     virtual void setCameraOrientation(int camera_id, const msr::airlib::Quaternionr& orientation) override;
 
@@ -57,4 +58,4 @@ class CarPawnApi : public msr::airlib::CarApiBase {
     UWheeledVehicleMovementComponent* movement_;
     bool api_control_enabled_ = false;
     CarControls last_controls_;
-};
+};

Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp

@@ -278,6 +278,13 @@ Pose MultiRotorConnector::getActorPose(const std::string& actor_name)
     return pose;
 }
 
+void MultiRotorConnector::setActorPose(const std::string& actor_name, const Pose& pose)
+{
+    UAirBlueprintLib::RunCommandOnGameThread([&pose, &actor_name, this]() {
+        wrapper_->setActorPose(actor_name, pose);
+    }, true);
+}
+
 bool MultiRotorConnector::setSegmentationObjectID(const std::string& mesh_name, int object_id,
     bool is_name_regex)
 {

Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h

@@ -62,6 +62,7 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase
 
     virtual void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) override;
     virtual Pose getActorPose(const std::string& actor_name) override;
+    virtual void setActorPose(const std::string& actor_name, const Pose& pose) override;
     virtual CameraInfo getCameraInfo(int camera_id) const override;
     virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) override;
 

Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp

@@ -408,4 +408,13 @@ msr::airlib::Pose VehiclePawnWrapper::getActorPose(std::string actor_name)
         : Pose::nanPose();
 }
 
-
+void VehiclePawnWrapper::setActorPose(std::string actor_name, const msr::airlib::Pose& pose)
+{
+    AActor* actor = UAirBlueprintLib::FindActor<AActor>(pawn_, FString(actor_name.c_str()));
+    if (actor)
+    {
+        FVector location = ned_transform_.toNeuUU(pose.position);
+        FQuat quat = ned_transform_.toFQuat(pose.orientation, true);
+        actor->SetActorLocationAndRotation(location, quat, false, nullptr, ETeleportType::TeleportPhysics);
+    }
+}

Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h

@@ -84,6 +84,7 @@ class VehiclePawnWrapper
     const WrapperConfig& getConfig() const;
 
     msr::airlib::Pose getActorPose(std::string actor_name);
+    void setActorPose(std::string actor_name, const msr::airlib::Pose& pose);
     std::string getVehicleConfigName() const;
 
     int getRemoteControlID() const;