diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 7cd0e5aad7..97ad480482 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/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); diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index 5bb5656589..ead8a857f4 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/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; diff --git a/AirLib/include/controllers/VehicleConnectorBase.hpp b/AirLib/include/controllers/VehicleConnectorBase.hpp index 3b1fd07a8c..82342b58f3 100644 --- a/AirLib/include/controllers/VehicleConnectorBase.hpp +++ b/AirLib/include/controllers/VehicleConnectorBase.hpp @@ -29,6 +29,7 @@ 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; @@ -36,4 +37,4 @@ class VehicleConnectorBase : public UpdatableObject }} //namespace -#endif \ No newline at end of file +#endif diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index 393a3b7ec3..2b197826b4 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/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); diff --git a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp b/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp index 89af35c354..9933908d25 100644 --- a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp +++ b/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 \ No newline at end of file +#endif diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index a31b124a26..9843f2ed80 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/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().to(); } +void RpcLibClientBase::simSetObjectPose(const std::string& object_name, const Pose& pose) +{ + pimpl_->client.call("simSetObjectPose", object_name, RpcLibAdapatorsBase::Pose(pose)); +} }} //namespace diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index fff57ca485..ae328f4c4e 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/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); diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 35bfe780e6..a72d0c101d 100644 --- a/PythonClient/AirSimClient.py +++ b/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 diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index a0d9b27ed4..cb8369e4eb 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/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_; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h index 51cb35c1a7..67dc51e191 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h +++ b/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_; -}; \ No newline at end of file +}; diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp index 4b644c0714..d5739ea319 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp +++ b/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) { diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h index b4e6f67eaa..8a8897170f 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h +++ b/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; diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index e3d72569e8..edec907f71 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/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(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); + } +} diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h index 7d7cff8d2d..a93bcb0cdd 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h +++ b/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;