From 4f76654df36d2c5585aed8a46017339c0032ef27 Mon Sep 17 00:00:00 2001 From: Xiaocheng Song Date: Thu, 2 Apr 2020 01:25:32 +0800 Subject: [PATCH 1/2] [plot_apis] multirotor: add api for setting trace line's color and thickness --- AirLib/include/api/RpcLibClientBase.hpp | 1 + AirLib/include/api/VehicleSimApiBase.hpp | 1 + AirLib/src/api/RpcLibClientBase.cpp | 5 +++ AirLib/src/api/RpcLibServerBase.cpp | 4 +++ PythonClient/airsim/client.py | 3 ++ PythonClient/multirotor/set_trace_line.py | 34 +++++++++++++++++++ .../AirsimWrapper/Source/PawnSimApi.cpp | 6 ++++ .../AirsimWrapper/Source/PawnSimApi.h | 1 + Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 8 ++++- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 4 +++ 10 files changed, 66 insertions(+), 1 deletion(-) create mode 100644 PythonClient/multirotor/set_trace_line.py diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 77bacf9e90..6dffae6222 100644 --- 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& color_rgba, float thickness=3.0f, const std::string& vehicle_name = ""); vector simGetImages(vector request, const std::string& vehicle_name = ""); vector 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 index 398d14954f..25b9579b03 100644 --- 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& 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 index 6e55cf78a3..536f392f13 100644 --- 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& color_rgba, float thickness, const std::string & vehicle_name) +{ + pimpl_->client.call("simSetTraceLine", color_rgba, thickness, vehicle_name); +} + vector RpcLibClientBase::simGetImages(vector 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 index bdca6ac1ae..2cb514e0de 100644 --- 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& 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 { return getVehicleApi(vehicle_name)->getLidarSegmentation(lidar_name); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 1528ac3d8f..94b8cd63ed 100644 --- 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 new file mode 100644 index 0000000000..f8a1a6f271 --- /dev/null +++ b/PythonClient/multirotor/set_trace_line.py @@ -0,0 +1,34 @@ +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 index a0d837268e..cacadce82f 100644 --- 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& 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 index af9f1fe3a0..0108cc01b5 100644 --- 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& 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 index c108b148da..0f3ab3946b 100644 --- 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& 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 index 67f3d39d57..c67922abfd 100644 --- 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& 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 kinematics_; std::unique_ptr environment_; + + FColor trace_color_ = FColor::Purple; + float trace_thickness_ = 3.0f; }; From 8d48bad5e84c7ef426113ff240ccfdf36e4da69a Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 14 Apr 2020 14:28:29 -0700 Subject: [PATCH 2/2] [plot_apis] traceline: add setting.json --- PythonClient/multirotor/set_trace_line.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/PythonClient/multirotor/set_trace_line.py b/PythonClient/multirotor/set_trace_line.py index f8a1a6f271..9324af8ec8 100644 --- a/PythonClient/multirotor/set_trace_line.py +++ b/PythonClient/multirotor/set_trace_line.py @@ -1,6 +1,18 @@ +# 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