added getObjectPose API

fish1996 · Dec 7, 2017 · c22e30a · c22e30a
1 parent ec2501e
commit c22e30a
Show file tree

Hide file tree

Showing 17 changed files with 115 additions and 24 deletions.
diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp
@@ -39,6 +39,8 @@ class RpcLibClientBase {
     int simGetSegmentationObjectID(const std::string& mesh_name);
     void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
 
+    Pose simGetObjectPose(const std::string& object_name);
+
     virtual ~RpcLibClientBase();    //required for pimpl
 
 protected:

diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp
@@ -26,10 +26,12 @@ class VehicleApiBase {
     virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0;
     virtual int simGetSegmentationObjectID(const std::string& mesh_name) = 0;
 
-    virtual void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) = 0;
+    virtual void simPrintLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0;
 
     virtual CollisionInfo getCollisionInfo() = 0;
 
+    virtual Pose simGetObjectPose(const std::string& object_name) = 0;
+
     virtual ~VehicleApiBase() = default;
 };
 

diff --git a/AirLib/include/controllers/VehicleConnectorBase.hpp b/AirLib/include/controllers/VehicleConnectorBase.hpp
@@ -32,6 +32,7 @@ class VehicleConnectorBase : public UpdatableObject
         bool is_name_regex = false) = 0;
     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;
 
 };
 

diff --git a/AirLib/include/vehicles/multirotor/api/DroneApi.hpp b/AirLib/include/vehicles/multirotor/api/DroneApi.hpp
@@ -268,11 +268,17 @@ class DroneApi : public VehicleApiBase {
             return vector<uint8_t>();
     }
 
-    virtual void simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) override
+    virtual void simPrintLogMessage(const std::string& message, const std::string& message_param, unsigned char severity) override
     {
         vehicle_->printLogMessage(message, message_param, severity);
     }
 
+    virtual Pose simGetObjectPose(const std::string& actor_name) override
+    {
+        return vehicle_->getActorPose(actor_name);
+    }
+
+
     virtual bool isApiControlEnabled() const override
     {
         return controller_->isApiControlEnabled();

diff --git a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp b/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp
@@ -77,6 +77,12 @@ class RealMultirotorConnector : public VehicleConnectorBase
         unused(severity);
     }
 
+    virtual Pose getActorPose(const std::string& actor_name) override
+    {
+        unused(actor_name);
+    }
+
+
 private:
     VehicleControllerBase* controller_;
 };

diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp
@@ -155,6 +155,11 @@ CollisionInfo RpcLibClientBase::getCollisionInfo()
     return pimpl_->client.call("getCollisionInfo").as<RpcLibAdapatorsBase::CollisionInfo>().to();
 }
 
+msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name)
+{
+    return pimpl_->client.call("simGetObjectPose").as<RpcLibAdapatorsBase::Pose>().to();
+}
+
 
 }} //namespace
 

diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp
@@ -73,9 +73,9 @@ RpcLibServerBase::RpcLibServerBase(VehicleApiBase* vehicle, string server_addres
         bind("simSetPose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision) -> void {
         vehicle_->simSetPose(pose.to(), ignore_collision);
     });
-    pimpl_->server.
-        bind("simGetPose", [&]() ->
-            RpcLibAdapatorsBase::Pose { return vehicle_->simGetPose();
+    pimpl_->server.bind("simGetPose", [&]() -> RpcLibAdapatorsBase::Pose { 
+        const auto& pose = vehicle_->simGetPose();
+        return RpcLibAdapatorsBase::Pose(pose);
     });
 
     pimpl_->server.
@@ -91,19 +91,28 @@ RpcLibServerBase::RpcLibServerBase(VehicleApiBase* vehicle, string server_addres
         vehicle_->reset();
     });
 
-    pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, std::string message_param, unsigned char severity) -> void {
+    pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void {
         vehicle_->simPrintLogMessage(message, message_param, severity);
     });
 
     pimpl_->server.bind("getHomeGeoPoint", [&]() -> RpcLibAdapatorsBase::GeoPoint {
-        return vehicle_->getHomeGeoPoint();
+        const auto& geo_point = vehicle_->getHomeGeoPoint();
+        return RpcLibAdapatorsBase::GeoPoint(geo_point);
     });
 
     pimpl_->server.bind("enableApiControl", [&](bool is_enabled) -> void { vehicle_->enableApiControl(is_enabled); });
     pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return vehicle_->isApiControlEnabled(); });
 
-    pimpl_->server.bind("getCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { return vehicle_->getCollisionInfo(); });
+    pimpl_->server.bind("getCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { 
+        const auto& collision_info = vehicle_->getCollisionInfo(); 
+        return RpcLibAdapatorsBase::CollisionInfo(collision_info);
+    });
 
+    pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { 
+        const auto& pose = vehicle_->simGetObjectPose(object_name); 
+        return RpcLibAdapatorsBase::Pose(pose);
+    });
+
     pimpl_->server.suppress_exceptions(true);
 }
 

diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py
@@ -66,7 +66,7 @@ class Pose(MsgpackMixin):
     position = Vector3r()
     orientation = Quaternionr()
 
-    def __init__(self, position_val, orientation_val):
+    def __init__(self, position_val = Vector3r(), orientation_val = Quaternionr()):
         self.position = position_val
         self.orientation = orientation_val
 
@@ -180,6 +180,9 @@ def simGetSegmentationObjectID(self, mesh_name):
         return self.client.call('simGetSegmentationObjectID', mesh_name)
     def simPrintLogMessage(self, message, message_param = "", severity = 0):
         return self.client.call('simPrintLogMessage', message, message_param, severity)
+    def simGetObjectPose(self, object_name):
+        pose = self.client.call('simGetObjectPose', object_name)
+        return Pose.from_msgpack(pose)
 
 
     # camera control
@@ -241,7 +244,8 @@ def simSetPose(self, pose, ignore_collison):
         self.client.call('simSetPose', pose, ignore_collison)
 
     def simGetPose(self):
-        return self.client.call('simGetPose')
+        pose = self.client.call('simGetPose')
+        return Pose.from_msgpack(pose)
 
     # helper method for converting getOrientation to roll/pitch/yaw
     # https:#en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj
@@ -12,8 +12,10 @@
     <OutputPath>.</OutputPath>
     <Name>PythonClient</Name>
     <RootNamespace>PythonClient</RootNamespace>
-    <InterpreterId>{9a7a9026-48c1-4688-9d5d-e5699d47d074}</InterpreterId>
-    <InterpreterVersion>3.5</InterpreterVersion>
+    <InterpreterId>
+    </InterpreterId>
+    <InterpreterVersion>
+    </InterpreterVersion>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)' == 'Debug' ">
     <DebugSymbols>true</DebugSymbols>
@@ -42,6 +44,9 @@
     <Compile Include="drone_stress_test.py">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="objects.py">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="seg_pallete.py">
       <SubType>Code</SubType>
     </Compile>
@@ -66,9 +71,6 @@
       <SubType>Code</SubType>
     </Compile>
   </ItemGroup>
-  <ItemGroup>
-    <InterpreterReference Include="{9a7a9026-48c1-4688-9d5d-e5699d47d074}\3.5" />
-  </ItemGroup>
   <PropertyGroup>
     <VisualStudioVersion Condition="'$(VisualStudioVersion)' == ''">10.0</VisualStudioVersion>
   </PropertyGroup>

diff --git a/PythonClient/objects.py b/PythonClient/objects.py
@@ -0,0 +1,12 @@
+from AirSimClient import *
+import pprint
+
+
+client = MultirotorClient()
+client.confirmConnection()
+
+p = Pose(Vector3r(), Quaternionr())
+
+pose = client.simGetObjectPose("TemplateCube_Rounded_17");
+print("Position: %s, Orientation: %s" % (pprint.pformat(pose.position),
+    pprint.pformat(pose.orientation)))
diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp
@@ -98,12 +98,12 @@ T* UAirBlueprintLib::FindActor(const UObject* context, FString name)
     FindAllActor<T>(context, foundActors);
 
     for (AActor* actor : foundActors) {
-        if (actor->GetName().Compare(name) == 0) {
+        if (actor->GetActorLabel().Compare(name) == 0 || actor->GetName().Compare(name) == 0) {
             return static_cast<T*>(actor);
         }
     }
 
-    UAirBlueprintLib::LogMessage(name + TEXT(" Actor not found!"), TEXT(""), LogDebugLevel::Failure);
+    //UAirBlueprintLib::LogMessage(name + TEXT(" Actor not found!"), TEXT(""), LogDebugLevel::Failure);
     return nullptr;
 }
 

diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp
@@ -30,7 +30,7 @@ bool CarPawnApi::simSetSegmentationObjectID(const std::string& mesh_name, int ob
     return success;
 }
 
-void CarPawnApi::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity)
+void CarPawnApi::simPrintLogMessage(const std::string& message, const std::string& message_param, unsigned char severity)
 {
     pawn_->printLogMessage(message, message_param, severity);
 }
@@ -73,6 +73,17 @@ void CarPawnApi::setCarControls(const CarApiBase::CarControls& controls)
     movement_->SetUseAutoGears(!controls.is_manual_gear);
 }
 
+msr::airlib::Pose CarPawnApi::simGetObjectPose(const std::string& actor_name)
+{
+    msr::airlib::Pose pose;
+
+    UAirBlueprintLib::RunCommandOnGameThread([&pose, &actor_name, this]() {
+        pose = pawn_->getActorPose(actor_name);
+    }, true);
+
+    return pose;
+}
+
 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
@@ -18,7 +18,7 @@ class CarPawnApi : public msr::airlib::CarApiBase {
     virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id,
         bool is_name_regex = false) override;
 
-    virtual void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) override;
+    virtual void simPrintLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override;
 
     virtual int simGetSegmentationObjectID(const std::string& mesh_name) override;
 
@@ -45,6 +45,9 @@ class CarPawnApi : public msr::airlib::CarApiBase {
 
     virtual const CarApiBase::CarControls& getCarControls() const;
 
+    virtual msr::airlib::Pose simGetObjectPose(const std::string& actor_name);
+
+
 
     virtual ~CarPawnApi();
 

diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp
@@ -250,6 +250,18 @@ Pose MultiRotorConnector::getPose()
     return vehicle_.getPose();
 }
 
+Pose MultiRotorConnector::getActorPose(const std::string& actor_name)
+{
+    msr::airlib::Pose pose;
+
+    UAirBlueprintLib::RunCommandOnGameThread([&pose, &actor_name, this]() {
+        pose = vehicle_pawn_wrapper_->getActorPose(actor_name);
+    }, true);
+
+    return pose;
+}
+
+
 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
@@ -63,6 +63,7 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase
     virtual msr::airlib::ImageCaptureBase* getImageCapture() override;
 
     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;
 
 
 private:

diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp
@@ -261,16 +261,21 @@ void VehiclePawnWrapper::plot(std::istream& s, FColor color, const Vector3r& off
 
 }
 
-void VehiclePawnWrapper::printLogMessage(const std::string& message, std::string message_param, unsigned char severity)
+void VehiclePawnWrapper::printLogMessage(const std::string& message, const std::string& message_param, unsigned char severity)
 {
     UAirBlueprintLib::LogMessageString(message, message_param, static_cast<LogDebugLevel>(severity));
 }
 
 //parameters in NED frame
 VehiclePawnWrapper::Pose VehiclePawnWrapper::getPose() const
 {
-    const Vector3r& position = NedTransform::toNedMeters(getPosition());
-    const Quaternionr& orientation = NedTransform::toQuaternionr(pawn_->GetActorRotation().Quaternion(), true);
+    return toPose(getPosition(), pawn_->GetActorRotation().Quaternion());
+}
+
+VehiclePawnWrapper::Pose VehiclePawnWrapper::toPose(const FVector& u_position, const FQuat& u_quat)
+{
+    const Vector3r& position = NedTransform::toNedMeters(u_position);
+    const Quaternionr& orientation = NedTransform::toQuaternionr(u_quat, true);
     return Pose(position, orientation);
 }
 
@@ -313,7 +318,7 @@ void VehiclePawnWrapper::setDebugPose(const Pose& debug_pose)
         FVector debug_position = state_.current_debug_position - state_.debug_position_offset;
         if ((state_.last_debug_position - debug_position).SizeSquared() > 0.25) {
             UKismetSystemLibrary::DrawDebugLine(pawn_->GetWorld(), state_.last_debug_position, debug_position, FColor(0xaa, 0x33, 0x11), -1, 10.0F);
-            UAirBlueprintLib::LogMessage("Debug Pose: ", debug_position.ToCompactString(), LogDebugLevel::Informational);
+            UAirBlueprintLib::LogMessage(FString("Debug Pose: "), debug_position.ToCompactString(), LogDebugLevel::Informational);
             state_.last_debug_position = debug_position;
         }
     }
@@ -342,5 +347,11 @@ std::string VehiclePawnWrapper::getLogLine()
     return log_line_;
 }
 
+msr::airlib::Pose VehiclePawnWrapper::getActorPose(std::string actor_name)
+{
+    AActor* actor = UAirBlueprintLib::FindActor<AActor>(pawn_, FString(actor_name.c_str()));
+    return actor ? toPose(actor->GetActorLocation(), actor->GetActorQuat())
+        : Pose::nanPose();
+}
 
 
diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h
@@ -68,11 +68,15 @@ class VehiclePawnWrapper
     void setLogLine(std::string line);
     std::string getLogLine();
 
-    void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
+    void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0);
 
     WrapperConfig& getConfig();
     const WrapperConfig& getConfig() const;
 
+    static VehiclePawnWrapper::Pose toPose(const FVector& u_position, const FQuat& u_quat);
+    msr::airlib::Pose getActorPose(std::string actor_name);
+
+
 protected:
     UPROPERTY(VisibleAnywhere)
         UParticleSystem* collision_display_template;