Merge pull request #2506 from songxiaocheng/PR/set_traceline

Features: add an api for setting trace line color and thickness
microsoft · Apr 14, 2020 · a8a2515 · a8a2515
2 parents 20f5bcd + 8d48bad
commit a8a2515
Show file tree

Hide file tree

Showing 10 changed files with 78 additions and 1 deletion.
diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp
@@ -88,6 +88,7 @@ class RpcLibClientBase {
 
     Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
     void simSetVehiclePose(const Pose& pose, bool ignore_collision, 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 = "");
     vector<uint8_t> simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = "");

diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp
@@ -63,6 +63,7 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     virtual std::string getVehicleName() const = 0;
     virtual std::string getRecordFileLine(bool is_header_line) const = 0;
     virtual void toggleTrace() = 0;
+    virtual void setTraceLine(const std::vector<float>& color_rgba, float thickness) = 0;
 
     //use pointer here because of derived classes for VehicleSetting
     const AirSimSettings::VehicleSetting* getVehicleSetting() const

diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp
@@ -219,6 +219,11 @@ void RpcLibClientBase::simSetVehiclePose(const Pose& pose, bool ignore_collision
     pimpl_->client.call("simSetVehiclePose", RpcLibAdapatorsBase::Pose(pose), ignore_collision, 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", 

diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp
@@ -150,6 +150,10 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::Pose(pose);
     });
 
+    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);
+    });
+
     pimpl_->server.
         bind("simGetLidarSegmentation", [&](const std::string& lidar_name, const std::string& vehicle_name) -> std::vector<int> {
         return getVehicleApi(vehicle_name)->getLidarSegmentation(lidar_name);

diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py
@@ -129,6 +129,9 @@ def simGetVehiclePose(self, vehicle_name = ''):
         pose = self.client.call('simGetVehiclePose', vehicle_name)
         return Pose.from_msgpack(pose)
 
+    def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name = ''):
+        self.client.call('simSetTraceLine', color_rgba, thickness, vehicle_name)
+
     def simGetObjectPose(self, object_name):
         pose = self.client.call('simGetObjectPose', object_name)
         return Pose.from_msgpack(pose)

diff --git a/PythonClient/multirotor/set_trace_line.py b/PythonClient/multirotor/set_trace_line.py
@@ -0,0 +1,46 @@
+# Please add "EnableTrace": true to your setting.json as shown below
+
+# {
+#   "SettingsVersion": 1.2,
+#   "SimMode": "Multirotor",
+#   "Vehicles": {
+#       "Drone": {
+#           "VehicleType": "SimpleFlight",
+#           "EnableTrace": true
+#         }
+#     }
+# }
+
+import setup_path
+import airsim
+import time
+
+# connect to the AirSim simulator
+client = airsim.MultirotorClient()
+client.confirmConnection()
+client.enableApiControl(True)
+client.armDisarm(True)
+
+client.takeoffAsync().join()
+client.hoverAsync().join()
+
+vehicleControl = client.moveByVelocityAsync(1, 1, 0, 12)
+
+client.simSetTraceLine([1.0, 0.0, 0.0, 1.0], 5)
+time.sleep(2)
+client.simSetTraceLine([0.0, 1.0, 0.0, 0.8], 10)
+time.sleep(2)
+client.simSetTraceLine([0.0, 0.0, 1.0, 0.6], 20)
+time.sleep(2)
+client.simSetTraceLine([1.0, 1.0, 0.0, 0.4], 30)
+time.sleep(2)
+client.simSetTraceLine([0.0, 1.0, 1.0, 0.2], 40)
+time.sleep(2)
+client.simSetTraceLine([1.0, 0.0, 1.0, 0.1], 50)
+time.sleep(2)
+
+vehicleControl.join()
+
+client.armDisarm(False)
+client.takeoffAsync().join()
+client.enableApiControl(False)
diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp
@@ -183,6 +183,12 @@ void PawnSimApi::toggleTrace()
 	state_.tracing_enabled = !state_.tracing_enabled;
 }
 
+void PawnSimApi::setTraceLine(const std::vector<float>& color_rgba, float thickness)
+{
+    throw std::invalid_argument(common_utils::Utils::stringf(
+        "setTraceLine is not supported on unity").c_str());
+}
+
 void PawnSimApi::allowPassthroughToggleInput()
 {
 	state_.passthrough_enabled = !state_.passthrough_enabled;

diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h
@@ -82,6 +82,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
 		return params_.vehicle_name;
 	}
 	virtual void toggleTrace() override;
+	virtual void setTraceLine(const std::vector<float>& color_rgba, float thickness) override;
 	virtual void updateRenderedState(float dt) override;
 	virtual void updateRendering(float dt) override;
 	virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override;

diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp
@@ -363,6 +363,12 @@ void PawnSimApi::toggleTrace()
     }
 }
 
+void PawnSimApi::setTraceLine(const std::vector<float>& color_rgba, float thickness) {
+    FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]};
+    trace_color_ = color.ToFColor(true);
+    trace_thickness_ = thickness;
+}
+
 void PawnSimApi::allowPassthroughToggleInput()
 {
     state_.passthrough_enabled = !state_.passthrough_enabled;
@@ -463,7 +469,7 @@ void PawnSimApi::setPoseInternal(const Pose& pose, bool ignore_collision)
         params_.pawn->SetActorLocationAndRotation(position, orientation, true);
 
     if (state_.tracing_enabled && (state_.last_position - position).SizeSquared() > 0.25) {
-        DrawDebugLine(params_.pawn->GetWorld(), state_.last_position, position, FColor::Purple, true, -1.0F, 0, 3.0F);
+        DrawDebugLine(params_.pawn->GetWorld(), state_.last_position, position, trace_color_, true, -1.0F, 0, trace_thickness_);
         state_.last_position = position;
     }
     else if (!state_.tracing_enabled) {

diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h
@@ -87,6 +87,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase {
         return params_.vehicle_name;
     }
     virtual void toggleTrace() override;
+    virtual void setTraceLine(const std::vector<float>& color_rgba, float thickness) override;
 
     virtual void updateRenderedState(float dt) override;
     virtual void updateRendering(float dt) override;
@@ -189,4 +190,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase {
 
     std::unique_ptr<msr::airlib::Kinematics> kinematics_;
     std::unique_ptr<msr::airlib::Environment> environment_;
+
+    FColor trace_color_ = FColor::Purple;
+    float trace_thickness_ = 3.0f;
 };