From c5bd0a386ab83119d14dc4a6022cc694623869c1 Mon Sep 17 00:00:00 2001 From: madratman Date: Fri, 8 Nov 2019 07:39:04 -0800 Subject: [PATCH 1/6] [plotting] add RViz like plot APIs for debugging --- AirLib/include/api/RpcLibClientBase.hpp | 8 +++ AirLib/include/api/WorldSimApiBase.hpp | 10 +++ AirLib/src/api/RpcLibClientBase.cpp | 55 +++++++++++++++++ AirLib/src/api/RpcLibServerBase.cpp | 42 +++++++++++++ PythonClient/airsim/client.py | 25 ++++++++ PythonClient/computer_vision/plot_markers.py | 36 +++++++++++ Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 65 ++++++++++++++++++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 10 +++ 8 files changed, 251 insertions(+) create mode 100644 PythonClient/computer_vision/plot_markers.py diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 54c30d671a..3f53fecee1 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -60,6 +60,14 @@ class RpcLibClientBase { int simGetSegmentationObjectID(const std::string& mesh_name) const; void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0); + void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent); + void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); + void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); + void simPlotArrowList(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent); + void simPlotTransform(const vector& poses, float scale, float thickness, float duration, bool is_persistent); + // void simPlotTransformAndName(const vector& poses, const vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent); + void simPlotStrings(const vector& positions, const vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent); + // void simPlotStringOnActor(const vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent); bool armDisarm(bool arm, const std::string& vehicle_name = ""); bool isApiControlEnabled(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 4276682c03..d62ab4588f 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -42,6 +42,16 @@ class WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; + //----------- Plotting APIs ----------/ + virtual void simPlotPoints(const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) = 0; + virtual void simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotArrowList(const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0; + virtual void simPlotTransform(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) = 0; + // virtual void simPlotTransformAndName(const std::vector& poses, const std::vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) = 0; + virtual void simPlotStrings(const std::vector& position, const std::vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) = 0; + // virtual void simPlotStringOnActor(const std::vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) = 0; + virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; virtual Pose getObjectPose(const std::string& object_name) const = 0; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index a7b8dba397..abeb6b3476 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -242,6 +242,61 @@ void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::strin pimpl_->client.call("simPrintLogMessage", message, message_param, severity); } +void RpcLibClientBase::simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotPoints", conv_points, color_rgba, size, duration, is_persistent); +} + +void RpcLibClientBase::simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotLineStrip", conv_points, color_rgba, thickness, duration, is_persistent); +} + +void RpcLibClientBase::simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotLineList", conv_points, color_rgba, thickness, duration, is_persistent); +} + +void RpcLibClientBase::simPlotArrowList(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) +{ + vector conv_points_start; + RpcLibAdapatorsBase::from(points_start, conv_points_start); + vector conv_points_end; + RpcLibAdapatorsBase::from(points_end, conv_points_end); + pimpl_->client.call("simPlotArrowList", conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); + +} + +void RpcLibClientBase::simPlotTransform(const vector& poses, float scale, float thickness, float duration, bool is_persistent) +{ + vector conv_poses; + RpcLibAdapatorsBase::from(poses, conv_poses); + pimpl_->client.call("simPlotTransform", conv_poses, scale, thickness, duration, is_persistent); +} + +// void RpcLibClientBase::simPlotTransformAndNames(const vector& poses, const vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) +// { + +// } + +void RpcLibClientBase::simPlotStrings(const vector& positions, const vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) +{ + vector conv_positions; + RpcLibAdapatorsBase::from(positions, conv_positions); + pimpl_->client.call("simPlotStrings", conv_positions, strings, scale, color_rgba, duration, is_persistent); +} + +// void RpcLibClientBase::simPlotStringOnActor(const vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) +// { + +// } + bool RpcLibClientBase::simIsPaused() const { return pimpl_->client.call("simIsPaused").as(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index ffa06f197d..169d51b3e9 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -242,6 +242,48 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return getWorldSimApi()->setObjectPose(object_name, pose.to(), teleport); }); + pimpl_->server.bind("simPlotPoints", [&](const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, duration, is_persistent); + }); + pimpl_->server.bind("simPlotLineStrip", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, duration, is_persistent); + }); + pimpl_->server.bind("simPlotLineList", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, duration, is_persistent); + }); + pimpl_->server.bind("simPlotArrowList", [&](const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) -> void { + vector conv_points_start; + RpcLibAdapatorsBase::to(points_start, conv_points_start); + vector conv_points_end; + RpcLibAdapatorsBase::to(points_end, conv_points_end); + getWorldSimApi()->simPlotArrowList(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); + }); + pimpl_->server.bind("simPlotTransform", [&](const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) -> void { + vector conv_poses; + RpcLibAdapatorsBase::to(poses, conv_poses); + getWorldSimApi()->simPlotTransform(conv_poses, scale, thickness, duration, is_persistent); + }); + // pimpl_->server.bind("simPlotTransformAndNames", [&](const std::vector& poses, const std::vector names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) -> void { + // vector conv_poses; + // RpcLibAdapatorsBase::to(poses, conv_poses); + // getWorldSimApi()->simPlotTransformAndNames(conv_poses, names, tf_scale, text_scale, text_color, duration, is_persistent); + // }); + pimpl_->server.bind("simPlotStrings", [&](const std::vector& positions, const std::vector strings, float scale, const vector& color_rgba, float duration, bool is_persistent) -> void { + vector conv_positions; + RpcLibAdapatorsBase::to(positions, conv_positions); + getWorldSimApi()->simPlotStrings(conv_positions, strings, scale, color_rgba, duration, is_persistent); + }); + // pimpl_->server.bind("simPlotStringOnActor", [&](const std::vector& positions, const std::vector strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) -> void { + // vector conv_positions; + // RpcLibAdapatorsBase::to(positions, conv_positions); + // getWorldSimApi()->simPlotStringOnActor(conv_positions, strings, scale, color_rgba, duration, is_persistent); + // }); pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState { const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics(); return RpcLibAdapatorsBase::KinematicsState(result); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 3bfddb9f30..a5962cc7c4 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -168,6 +168,31 @@ def getLidarData(self, lidar_name = '', vehicle_name = ''): def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name) + # Plotting APIs + def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], size = 10, duration = -1.0, is_persistent = False): + return self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent) + + def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): + return self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent) + + def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): + return self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent) + + def simPlotArrowList(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, arrow_size = 2, duration = -1.0, is_persistent = False): + return self.client.call('simPlotArrowList', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) + + def simPlotTransform(self, poses, scale = 5, thickness = 5, duration = -1.0, is_persistent = False): + return self.client.call('simPlotTransform', poses, scale, thickness, duration, is_persistent) + + # def simPlotTransformAndNames(self, poses, names, tf_scale = 5, text_scale = 10, text_color = [1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): + # return self.client.call('simPlotTransformAndNames', poses, names, tf_scale, text_scale, duration, is_persistent) + + def simPlotStrings(self, positions, strings = ["Microsoft", "AirSim"], scale = 5, color_rgba=[1.0, 0.0, 0.0, 0.4], duration = -1.0, is_persistent = False): + return self.client.call('simPlotStrings', positions, strings, scale, color_rgba, duration, is_persistent) + + # def simPlotStringOnActor(self, positions, strings = ["Microsoft", "AirSim"], actor_name = "", scale = 5, color_rgba=[1.0, 0.0, 0.0, 0.4], duration = -1.0, is_persistent = False): + # return self.client.call('simPlotStringOnActor', positions, strings, scale, actor_name, color_rgba, duration, is_persistent) + #----------- APIs to control ACharacter in scene ----------/ def simCharSetFaceExpression(self, expression_name, value, character_name = ""): self.client.call('simCharSetFaceExpression', expression_name, value, character_name) diff --git a/PythonClient/computer_vision/plot_markers.py b/PythonClient/computer_vision/plot_markers.py new file mode 100644 index 0000000000..65c2829e1a --- /dev/null +++ b/PythonClient/computer_vision/plot_markers.py @@ -0,0 +1,36 @@ +import setup_path +import airsim +from airsim import Vector3r +import numpy as np + +client = airsim.VehicleClient() +client.confirmConnection() + +# plot 2 red arrows for 3 seconds +arrow_1_start, arrow_2_start = Vector3r(0,0,-1), Vector3r(0,0,-3) +arrow_1_end, arrow_2_end = Vector3r(1,1,-1), Vector3r(2,2,-3) +client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], + color_rgba = [1.0, 0.0, 1.0, 1.0], duration = 3.0, arrow_size = 20, thickness = 4) + +# plot 2 yellow arrows for 4 seconds +arrow_1_start, arrow_2_start = Vector3r(0,1,-1), Vector3r(0,1,-3) +arrow_1_end, arrow_2_end = Vector3r(4,5,-1), Vector3r(2,3,-3) +client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], + color_rgba = [1.0, 1.0, 0.0, 1.0], duration = 4.0, arrow_size = 20, thickness = 3) + +# plot 2 red arrows for 5 seconds +arrow_1_start, arrow_2_start = Vector3r(1,1,-2), Vector3r(1,1,-4) +arrow_1_end, arrow_2_end = Vector3r(-4,-4,-2), Vector3r(-2,-2,-4) +client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], + color_rgba = [1.0, 0.0, 0.0, 1.0], duration = 5.0, arrow_size = 20, thickness = 2) + +# plot 2 white arrows which are persistent +arrow_1_start, arrow_2_start = Vector3r(2,2,-2), Vector3r(0,1,-4) +arrow_1_end, arrow_2_end = Vector3r(2,3,-2), Vector3r(2,4,-4) +client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], + color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 5.0, arrow_size = 20, thickness = 10, is_persistent = True) + +# plot points +client.simPlotPoints(points = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,10,20), np.linspace(0,20,20))], color_rgba=[1.0, 0.0, 0.0, 1.0], size = 20, duration = 20.0, is_persistent = False) +client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,-10,20), np.linspace(0,-20,20), np.linspace(0,-5,20))], color_rgba=[0.0, 0.0, 1.0, 1.0], size = 10, duration = 20.0, is_persistent = False) +client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,-20,20), np.linspace(0,-7,20))], color_rgba=[1.0, 0.0, 1.0, 1.0], size = 15, duration = 20.0, is_persistent = False) diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 5120219f46..633039bb42 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -3,6 +3,7 @@ #include "TextureShuffleActor.h" #include "common/common_utils/Utils.hpp" #include "Weather/WeatherLib.h" +#include "DrawDebugHelpers.h" WorldSimApi::WorldSimApi(ASimModeBase* simmode) : simmode_(simmode) @@ -109,6 +110,7 @@ void WorldSimApi::enableWeather(bool enable) { UWeatherLib::setWeatherEnabled(simmode_->GetWorld(), enable); } + void WorldSimApi::setWeatherParameter(WeatherParameter param, float val) { unsigned char param_n = static_cast(msr::airlib::Utils::toNumeric(param)); @@ -154,6 +156,69 @@ std::unique_ptr> WorldSimApi::swapTextures(const std::s }, true); return swappedObjectNames; } +//----------- Plotting APIs ----------/ +void WorldSimApi::simPlotPoints(const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) +{ + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (const auto& point : points) + { + DrawDebugPoint(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(point), size, color.ToFColor(true), is_persistent, duration); + } +} + +// plot line for points 0-1, 1-2, 2-3 +void WorldSimApi::simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +{ + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (size_t idx = 0; idx != points.size()-1; idx++) + { + DrawDebugLine(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx]), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx+1]), color.ToFColor(true), is_persistent, duration, 0, thickness); + } +} + +// plot line for points 0-1, 2-3, 4-5... must be even number of points +void WorldSimApi::simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +{ + if (points.size() % 2) + { + + } + + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (int idx = 0; idx < points.size(); idx += 2) + { + DrawDebugLine(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx]), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx+1]), color.ToFColor(true), is_persistent, duration, 0, thickness); + } +} + +void WorldSimApi::simPlotArrowList(const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) +{ + // assert points_start.size() == poinst_end.size() + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (int idx = 0; idx < points_start.size(); idx += 1) + { + DrawDebugDirectionalArrow(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(points_start[idx]), simmode_->getGlobalNedTransform().fromGlobalNed(points_end[idx]), arrow_size, color.ToFColor(true), is_persistent, duration, 0, thickness); + } +} + +void WorldSimApi::simPlotTransform(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) +{ + for (const auto& pose : poses) + { + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).Rotator(), scale, is_persistent, duration, 0, thickness); + } +} +// void WorldSimApi::simPlotTransformAndName(const std::vector& poses, const std::vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent); +void WorldSimApi::simPlotStrings(const std::vector& positions, const std::vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) +{ + // assert positions.size() == strings.size() + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (int idx = 0; idx < positions.size(); idx += 1) + { + DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(positions[idx]), FString(strings[idx].c_str()), NULL, color.ToFColor(true), duration, false, scale); + } +} +// void WorldSimApi::simPlotStringOnActor(const std::vector& pose, const std::vector& strings, const std::vector& actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent); //------------------------------------------------- Char APIs -----------------------------------------------------------/ diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 146426e275..2a91f52f37 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -36,6 +36,16 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual Pose getObjectPose(const std::string& object_name) const override; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; + //----------- Plotting APIs ----------/ + virtual void simPlotPoints(const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) override; + virtual void simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotArrowList(const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override; + virtual void simPlotTransform(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) override; + // virtual void simPlotTransformAndName(const std::vector& poses, const std::vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) override; + virtual void simPlotStrings(const std::vector& position, const std::vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) override; + // virtual void simPlotStringOnActor(const std::vector& pose, const std::string& strings, std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) override; + //----------- APIs to control ACharacter in scene ----------/ virtual void charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) override; virtual float charGetFaceExpression(const std::string& expression_name, const std::string& character_name) const override; From a81448157eca8e93e294c1c733a0fb0b599c3eee Mon Sep 17 00:00:00 2001 From: madratman Date: Sun, 10 Nov 2019 18:15:54 -0800 Subject: [PATCH 2/6] [plotting] add more examples, add docstrings to client.py, omit return vals from client.py --- PythonClient/airsim/client.py | 68 +++++++++++++++++--- PythonClient/computer_vision/plot_markers.py | 58 ++++++++++------- 2 files changed, 95 insertions(+), 31 deletions(-) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index a5962cc7c4..92f3187a9c 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -170,28 +170,80 @@ def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): # Plotting APIs def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], size = 10, duration = -1.0, is_persistent = False): - return self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent) + """ + Plot a list of 3D points in World NED frame + + Args: + points (list[Vector3r]): List of Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + size (int, optional): Size of plotted point + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent) def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): - return self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent) + """ + Plots a line strip in World NED frame, defined from points[0] to points[1], points[1] to points[2], ... , points[n-2] to points[n-1] + + Args: + points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (int, optional): Thickness of line + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent) def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): - return self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent) + """ + Plots a line strip in World NED frame, defined from points[0] to points[1], points[2] to points[3], ... , points[n-2] to points[n-1] + + Args: + points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects. Must be even + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (int, optional): Thickness of line + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent) def simPlotArrowList(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, arrow_size = 2, duration = -1.0, is_persistent = False): - return self.client.call('simPlotArrowList', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) + """ + Plots a list of arrows in World NED frame, defined from points_start[0] to points_end[0], points_start[1] to points_end[1], ... , points_start[n-1] to points_end[n-1] + + Args: + points_start (list[Vector3r]): List of 3D start positions of arrow start positions, specified as Vector3r objects + points_end (list[Vector3r]): List of 3D end positions of arrow start positions, specified as Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (int, optional): Thickness of line + arrow_size (int, optional): Size of arrow head + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotArrowList', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) def simPlotTransform(self, poses, scale = 5, thickness = 5, duration = -1.0, is_persistent = False): - return self.client.call('simPlotTransform', poses, scale, thickness, duration, is_persistent) + """ + Plots a list of transforms in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + scale (int, optional): Length of transform axes + thickness (int, optional): Thickness of lines + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotTransform', poses, scale, thickness, duration, is_persistent) # def simPlotTransformAndNames(self, poses, names, tf_scale = 5, text_scale = 10, text_color = [1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): - # return self.client.call('simPlotTransformAndNames', poses, names, tf_scale, text_scale, duration, is_persistent) + # self.client.call('simPlotTransformAndNames', poses, names, tf_scale, text_scale, duration, is_persistent) def simPlotStrings(self, positions, strings = ["Microsoft", "AirSim"], scale = 5, color_rgba=[1.0, 0.0, 0.0, 0.4], duration = -1.0, is_persistent = False): - return self.client.call('simPlotStrings', positions, strings, scale, color_rgba, duration, is_persistent) + self.client.call('simPlotStrings', positions, strings, scale, color_rgba, duration, is_persistent) # def simPlotStringOnActor(self, positions, strings = ["Microsoft", "AirSim"], actor_name = "", scale = 5, color_rgba=[1.0, 0.0, 0.0, 0.4], duration = -1.0, is_persistent = False): - # return self.client.call('simPlotStringOnActor', positions, strings, scale, actor_name, color_rgba, duration, is_persistent) + # self.client.call('simPlotStringOnActor', positions, strings, scale, actor_name, color_rgba, duration, is_persistent) #----------- APIs to control ACharacter in scene ----------/ def simCharSetFaceExpression(self, expression_name, value, character_name = ""): diff --git a/PythonClient/computer_vision/plot_markers.py b/PythonClient/computer_vision/plot_markers.py index 65c2829e1a..7c10dcdf37 100644 --- a/PythonClient/computer_vision/plot_markers.py +++ b/PythonClient/computer_vision/plot_markers.py @@ -1,36 +1,48 @@ import setup_path import airsim -from airsim import Vector3r +from airsim import Vector3r, Quaternionr, Pose +from airsim.utils import to_quaternion import numpy as np +import time client = airsim.VehicleClient() client.confirmConnection() -# plot 2 red arrows for 3 seconds -arrow_1_start, arrow_2_start = Vector3r(0,0,-1), Vector3r(0,0,-3) -arrow_1_end, arrow_2_end = Vector3r(1,1,-1), Vector3r(2,2,-3) -client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], - color_rgba = [1.0, 0.0, 1.0, 1.0], duration = 3.0, arrow_size = 20, thickness = 4) +# plot red arrows for 30 seconds +client.simPlotArrowList(points_start = [Vector3r(x,y,0) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], + points_end = [Vector3r(x,y,0) for x, y in zip(np.linspace(0,10,20), np.linspace(10,10,20))], + color_rgba = [1.0, 0.0, 1.0, 1.0], duration = 30.0, arrow_size = 10, thickness = 1) -# plot 2 yellow arrows for 4 seconds -arrow_1_start, arrow_2_start = Vector3r(0,1,-1), Vector3r(0,1,-3) -arrow_1_end, arrow_2_end = Vector3r(4,5,-1), Vector3r(2,3,-3) -client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], - color_rgba = [1.0, 1.0, 0.0, 1.0], duration = 4.0, arrow_size = 20, thickness = 3) +# plot magenta arrows for 15 seconds +client.simPlotArrowList(points_start = [Vector3r(x,y,-3) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], + points_end = [Vector3r(x,y,-5) for x, y in zip(np.linspace(0,10,20), np.linspace(10,20,20))], + color_rgba = [1.0, 1.0, 0.0, 1.0], duration = 15.0, arrow_size = 20, thickness = 3) -# plot 2 red arrows for 5 seconds -arrow_1_start, arrow_2_start = Vector3r(1,1,-2), Vector3r(1,1,-4) -arrow_1_end, arrow_2_end = Vector3r(-4,-4,-2), Vector3r(-2,-2,-4) -client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], - color_rgba = [1.0, 0.0, 0.0, 1.0], duration = 5.0, arrow_size = 20, thickness = 2) +# plot red arrows for 10 seconds +client.simPlotArrowList(points_start = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,0,20), np.linspace(-3,-10, 20))], + points_end = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(10,20,20), np.linspace(-5,-8, 20))], + color_rgba = [1.0, 0.0, 0.0, 1.0], duration = 10.0, arrow_size = 100, thickness = 5) # plot 2 white arrows which are persistent -arrow_1_start, arrow_2_start = Vector3r(2,2,-2), Vector3r(0,1,-4) -arrow_1_end, arrow_2_end = Vector3r(2,3,-2), Vector3r(2,4,-4) -client.simPlotArrowList(points_start = [arrow_1_start, arrow_2_start], points_end = [arrow_1_end, arrow_2_end], - color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 5.0, arrow_size = 20, thickness = 10, is_persistent = True) +client.simPlotArrowList(points_start = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,10,20), np.linspace(0,20,20))], + points_end = [Vector3r(x,y,-5) for x, y in zip(np.linspace(3,17,20), np.linspace(5,28,20))], + color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 5.0, arrow_size = 100, thickness = 1, is_persistent = True) # plot points -client.simPlotPoints(points = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,10,20), np.linspace(0,20,20))], color_rgba=[1.0, 0.0, 0.0, 1.0], size = 20, duration = 20.0, is_persistent = False) -client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,-10,20), np.linspace(0,-20,20), np.linspace(0,-5,20))], color_rgba=[0.0, 0.0, 1.0, 1.0], size = 10, duration = 20.0, is_persistent = False) -client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,-20,20), np.linspace(0,-7,20))], color_rgba=[1.0, 0.0, 1.0, 1.0], size = 15, duration = 20.0, is_persistent = False) +client.simPlotPoints(points = [Vector3r(x,y,-5) for x, y in zip(np.linspace(0,-10,20), np.linspace(0,-20,20))], color_rgba=[1.0, 0.0, 0.0, 1.0], size = 25, duration = 20.0, is_persistent = False) +client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,-10,20), np.linspace(0,-20,20), np.linspace(0,-5,20))], color_rgba=[0.0, 0.0, 1.0, 1.0], size = 10, duration = 20.0, is_persistent = False) +client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,-20,20), np.linspace(0,-7,20))], color_rgba=[1.0, 0.0, 1.0, 1.0], size = 15, duration = 20.0, is_persistent = False) + +# plot line strip. 0-1, 1-2, 2-3 +client.simPlotLineStrip(points = [Vector3r(x,y,-5) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5, duration = 30.0, is_persistent = False) + +# plot line list. 0-1, 2-3, 4-5. Must be even. +client.simPlotLineList(points = [Vector3r(x,y,-7) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5, duration = 30.0, is_persistent = False) + +# plot transforms +client.simPlotTransform(poses = [Pose(position_val=Vector3r(x,y,-7), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=np.pi/2)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], + scale = 35, thickness = 5, duration = 10.0, is_persistent = False) + +client.simPlotTransform(poses = [Pose(position_val=Vector3r(x,y,-5), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=0.0)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], + scale = 35, thickness = 5, duration = 10.0, is_persistent = False) + From cf176780643a7495e0bd141cf51571435a4cb30f Mon Sep 17 00:00:00 2001 From: madratman Date: Mon, 11 Nov 2019 18:00:24 -0800 Subject: [PATCH 3/6] [plotting] refactor/rename, finalize, finish plotting APIs --- AirLib/include/api/RpcLibClientBase.hpp | 9 ++- AirLib/include/api/WorldSimApiBase.hpp | 15 +++-- AirLib/src/api/RpcLibClientBase.cpp | 35 +++++------ AirLib/src/api/RpcLibServerBase.cpp | 33 +++++----- PythonClient/airsim/client.py | 64 +++++++++++++------- PythonClient/computer_vision/plot_markers.py | 14 +++-- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 34 +++++++---- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 15 +++-- 8 files changed, 120 insertions(+), 99 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 3f53fecee1..8d71d922de 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -63,11 +63,10 @@ class RpcLibClientBase { void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent); void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); - void simPlotArrowList(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent); - void simPlotTransform(const vector& poses, float scale, float thickness, float duration, bool is_persistent); - // void simPlotTransformAndName(const vector& poses, const vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent); - void simPlotStrings(const vector& positions, const vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent); - // void simPlotStringOnActor(const vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent); + void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent); + void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration); + void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent); + void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration); bool armDisarm(bool arm, const std::string& vehicle_name = ""); bool isApiControlEnabled(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index d62ab4588f..74d99de0f8 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -43,14 +43,13 @@ class WorldSimApiBase { const std::string& message_param = "", unsigned char severity = 0) = 0; //----------- Plotting APIs ----------/ - virtual void simPlotPoints(const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) = 0; - virtual void simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; - virtual void simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; - virtual void simPlotArrowList(const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0; - virtual void simPlotTransform(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) = 0; - // virtual void simPlotTransformAndName(const std::vector& poses, const std::vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) = 0; - virtual void simPlotStrings(const std::vector& position, const std::vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) = 0; - // virtual void simPlotStringOnActor(const std::vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) = 0; + virtual void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) = 0; + virtual void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0; + virtual void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) = 0; + virtual void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) = 0; virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; virtual Pose getObjectPose(const std::string& object_name) const = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index abeb6b3476..d055fc2900 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -263,39 +263,36 @@ void RpcLibClientBase::simPlotLineList(const vector& points, const vec pimpl_->client.call("simPlotLineList", conv_points, color_rgba, thickness, duration, is_persistent); } -void RpcLibClientBase::simPlotArrowList(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) +void RpcLibClientBase::simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) { vector conv_points_start; RpcLibAdapatorsBase::from(points_start, conv_points_start); vector conv_points_end; RpcLibAdapatorsBase::from(points_end, conv_points_end); - pimpl_->client.call("simPlotArrowList", conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); - + pimpl_->client.call("simPlotArrows", conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); } -void RpcLibClientBase::simPlotTransform(const vector& poses, float scale, float thickness, float duration, bool is_persistent) +void RpcLibClientBase::simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) +{ + vector conv_positions; + RpcLibAdapatorsBase::from(positions, conv_positions); + pimpl_->client.call("simPlotStrings", strings, conv_positions, scale, color_rgba, duration); +} + +void RpcLibClientBase::simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) { vector conv_poses; RpcLibAdapatorsBase::from(poses, conv_poses); - pimpl_->client.call("simPlotTransform", conv_poses, scale, thickness, duration, is_persistent); + pimpl_->client.call("simPlotTransforms", conv_poses, scale, thickness, duration, is_persistent); } -// void RpcLibClientBase::simPlotTransformAndNames(const vector& poses, const vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) -// { - -// } - -void RpcLibClientBase::simPlotStrings(const vector& positions, const vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) +void RpcLibClientBase::simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) { - vector conv_positions; - RpcLibAdapatorsBase::from(positions, conv_positions); - pimpl_->client.call("simPlotStrings", conv_positions, strings, scale, color_rgba, duration, is_persistent); -} + vector conv_poses; + RpcLibAdapatorsBase::from(poses, conv_poses); + pimpl_->client.call("simPlotTransformsWithNames", conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration); -// void RpcLibClientBase::simPlotStringOnActor(const vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) -// { - -// } +} bool RpcLibClientBase::simIsPaused() const { diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 169d51b3e9..261a985de1 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -257,33 +257,28 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& RpcLibAdapatorsBase::to(points, conv_points); getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, duration, is_persistent); }); - pimpl_->server.bind("simPlotArrowList", [&](const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) -> void { + pimpl_->server.bind("simPlotArrows", [&](const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) -> void { vector conv_points_start; RpcLibAdapatorsBase::to(points_start, conv_points_start); vector conv_points_end; RpcLibAdapatorsBase::to(points_end, conv_points_end); - getWorldSimApi()->simPlotArrowList(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); + getWorldSimApi()->simPlotArrows(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); }); - pimpl_->server.bind("simPlotTransform", [&](const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) -> void { - vector conv_poses; - RpcLibAdapatorsBase::to(poses, conv_poses); - getWorldSimApi()->simPlotTransform(conv_poses, scale, thickness, duration, is_persistent); - }); - // pimpl_->server.bind("simPlotTransformAndNames", [&](const std::vector& poses, const std::vector names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) -> void { - // vector conv_poses; - // RpcLibAdapatorsBase::to(poses, conv_poses); - // getWorldSimApi()->simPlotTransformAndNames(conv_poses, names, tf_scale, text_scale, text_color, duration, is_persistent); - // }); - pimpl_->server.bind("simPlotStrings", [&](const std::vector& positions, const std::vector strings, float scale, const vector& color_rgba, float duration, bool is_persistent) -> void { + pimpl_->server.bind("simPlotStrings", [&](const std::vector strings, const std::vector& positions, float scale, const vector& color_rgba, float duration) -> void { vector conv_positions; RpcLibAdapatorsBase::to(positions, conv_positions); - getWorldSimApi()->simPlotStrings(conv_positions, strings, scale, color_rgba, duration, is_persistent); + getWorldSimApi()->simPlotStrings(strings, conv_positions, scale, color_rgba, duration); + }); + pimpl_->server.bind("simPlotTransforms", [&](const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) -> void { + vector conv_poses; + RpcLibAdapatorsBase::to(poses, conv_poses); + getWorldSimApi()->simPlotTransforms(conv_poses, scale, thickness, duration, is_persistent); + }); + pimpl_->server.bind("simPlotTransformsWithNames", [&](const std::vector& poses, const std::vector names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) -> void { + vector conv_poses; + RpcLibAdapatorsBase::to(poses, conv_poses); + getWorldSimApi()->simPlotTransformsWithNames(conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration); }); - // pimpl_->server.bind("simPlotStringOnActor", [&](const std::vector& positions, const std::vector strings, const std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) -> void { - // vector conv_positions; - // RpcLibAdapatorsBase::to(positions, conv_positions); - // getWorldSimApi()->simPlotStringOnActor(conv_positions, strings, scale, color_rgba, duration, is_persistent); - // }); pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState { const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics(); return RpcLibAdapatorsBase::KinematicsState(result); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 92f3187a9c..2c45c3c36a 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -169,46 +169,46 @@ def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name) # Plotting APIs - def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], size = 10, duration = -1.0, is_persistent = False): + def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, duration = -1.0, is_persistent = False): """ Plot a list of 3D points in World NED frame Args: points (list[Vector3r]): List of Vector3r objects color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 - size (int, optional): Size of plotted point + size (float, optional): Size of plotted point duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent) - def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): + def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): """ Plots a line strip in World NED frame, defined from points[0] to points[1], points[1] to points[2], ... , points[n-2] to points[n-1] Args: points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 - thickness (int, optional): Thickness of line + thickness (float, optional): Thickness of line duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent) - def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): + def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): """ Plots a line strip in World NED frame, defined from points[0] to points[1], points[2] to points[3], ... , points[n-2] to points[n-1] Args: points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects. Must be even color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 - thickness (int, optional): Thickness of line + thickness (float, optional): Thickness of line duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent) - def simPlotArrowList(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 0.4], thickness = 5, arrow_size = 2, duration = -1.0, is_persistent = False): + def simPlotArrows(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, arrow_size = 2.0, duration = -1.0, is_persistent = False): """ Plots a list of arrows in World NED frame, defined from points_start[0] to points_end[0], points_start[1] to points_end[1], ... , points_start[n-1] to points_end[n-1] @@ -216,34 +216,54 @@ def simPlotArrowList(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, points_start (list[Vector3r]): List of 3D start positions of arrow start positions, specified as Vector3r objects points_end (list[Vector3r]): List of 3D end positions of arrow start positions, specified as Vector3r objects color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 - thickness (int, optional): Thickness of line - arrow_size (int, optional): Size of arrow head + thickness (float, optional): Thickness of line + arrow_size (float, optional): Size of arrow head duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ - self.client.call('simPlotArrowList', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) + self.client.call('simPlotArrows', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) - def simPlotTransform(self, poses, scale = 5, thickness = 5, duration = -1.0, is_persistent = False): + + def simPlotStrings(self, strings, positions, scale = 5, color_rgba=[1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of strings at desired positions in World NED frame. + + Args: + strings (list[String], optional): List of strings to plot + positions (list[Vector3r]): List of positions where the strings should be plotted. Should be in one-to-one correspondence with the strings' list + scale (float, optional): Font scale of transform name + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotStrings', strings, positions, scale, color_rgba, duration) + + def simPlotTransforms(self, poses, scale = 5.0, thickness = 5.0, duration = -1.0, is_persistent = False): """ Plots a list of transforms in World NED frame. Args: poses (list[Pose]): List of Pose objects representing the transforms to plot - scale (int, optional): Length of transform axes - thickness (int, optional): Thickness of lines + scale (float, optional): Length of transforms' axes + thickness (float, optional): Thickness of transforms' axes duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ - self.client.call('simPlotTransform', poses, scale, thickness, duration, is_persistent) + self.client.call('simPlotTransforms', poses, scale, thickness, duration, is_persistent) - # def simPlotTransformAndNames(self, poses, names, tf_scale = 5, text_scale = 10, text_color = [1.0, 0.0, 0.0, 0.4], thickness = 5, duration = -1.0, is_persistent = False): - # self.client.call('simPlotTransformAndNames', poses, names, tf_scale, text_scale, duration, is_persistent) - - def simPlotStrings(self, positions, strings = ["Microsoft", "AirSim"], scale = 5, color_rgba=[1.0, 0.0, 0.0, 0.4], duration = -1.0, is_persistent = False): - self.client.call('simPlotStrings', positions, strings, scale, color_rgba, duration, is_persistent) - - # def simPlotStringOnActor(self, positions, strings = ["Microsoft", "AirSim"], actor_name = "", scale = 5, color_rgba=[1.0, 0.0, 0.0, 0.4], duration = -1.0, is_persistent = False): - # self.client.call('simPlotStringOnActor', positions, strings, scale, actor_name, color_rgba, duration, is_persistent) + def simPlotTransformsWithNames(self, poses, names, tf_scale = 5.0, tf_thickness = 5.0, text_scale = 10.0, text_color_rgba = [1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of transforms with their names in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + names (list[string]): List of strings with one-to-one correspondence to list of poses + tf_scale (float, optional): Length of transforms' axes + tf_thickness (float, optional): Thickness of transforms' axes + text_scale (float, optional): Font scale of transform name + text_color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 for the transform name + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotTransformsWithNames', poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration) #----------- APIs to control ACharacter in scene ----------/ def simCharSetFaceExpression(self, expression_name, value, character_name = ""): diff --git a/PythonClient/computer_vision/plot_markers.py b/PythonClient/computer_vision/plot_markers.py index 7c10dcdf37..e423dcbbd9 100644 --- a/PythonClient/computer_vision/plot_markers.py +++ b/PythonClient/computer_vision/plot_markers.py @@ -9,22 +9,22 @@ client.confirmConnection() # plot red arrows for 30 seconds -client.simPlotArrowList(points_start = [Vector3r(x,y,0) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], +client.simPlotArrows(points_start = [Vector3r(x,y,0) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], points_end = [Vector3r(x,y,0) for x, y in zip(np.linspace(0,10,20), np.linspace(10,10,20))], color_rgba = [1.0, 0.0, 1.0, 1.0], duration = 30.0, arrow_size = 10, thickness = 1) # plot magenta arrows for 15 seconds -client.simPlotArrowList(points_start = [Vector3r(x,y,-3) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], +client.simPlotArrows(points_start = [Vector3r(x,y,-3) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], points_end = [Vector3r(x,y,-5) for x, y in zip(np.linspace(0,10,20), np.linspace(10,20,20))], color_rgba = [1.0, 1.0, 0.0, 1.0], duration = 15.0, arrow_size = 20, thickness = 3) # plot red arrows for 10 seconds -client.simPlotArrowList(points_start = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,0,20), np.linspace(-3,-10, 20))], +client.simPlotArrows(points_start = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,0,20), np.linspace(-3,-10, 20))], points_end = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(10,20,20), np.linspace(-5,-8, 20))], color_rgba = [1.0, 0.0, 0.0, 1.0], duration = 10.0, arrow_size = 100, thickness = 5) # plot 2 white arrows which are persistent -client.simPlotArrowList(points_start = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,10,20), np.linspace(0,20,20))], +client.simPlotArrows(points_start = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,10,20), np.linspace(0,20,20))], points_end = [Vector3r(x,y,-5) for x, y in zip(np.linspace(3,17,20), np.linspace(5,28,20))], color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 5.0, arrow_size = 100, thickness = 1, is_persistent = True) @@ -40,9 +40,11 @@ client.simPlotLineList(points = [Vector3r(x,y,-7) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5, duration = 30.0, is_persistent = False) # plot transforms -client.simPlotTransform(poses = [Pose(position_val=Vector3r(x,y,-7), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=np.pi/2)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], +client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,-7), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=np.pi/2)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], scale = 35, thickness = 5, duration = 10.0, is_persistent = False) -client.simPlotTransform(poses = [Pose(position_val=Vector3r(x,y,-5), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=0.0)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], +client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,-5), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=0.0)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], scale = 35, thickness = 5, duration = 10.0, is_persistent = False) +client.simPlotStrings(strings = ["Microsoft AirSim" for i in range(5)], positions = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,-5,5), np.linspace(0,-5,5))], + scale = 1, color_rgba=[1.0, 1.0, 1.0, 1.0], duration = 10.0) \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 633039bb42..4de9471ee7 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -157,7 +157,7 @@ std::unique_ptr> WorldSimApi::swapTextures(const std::s return swappedObjectNames; } //----------- Plotting APIs ----------/ -void WorldSimApi::simPlotPoints(const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) +void WorldSimApi::simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) { FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; for (const auto& point : points) @@ -167,7 +167,7 @@ void WorldSimApi::simPlotPoints(const std::vector& points, const vecto } // plot line for points 0-1, 1-2, 2-3 -void WorldSimApi::simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +void WorldSimApi::simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) { FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; for (size_t idx = 0; idx != points.size()-1; idx++) @@ -177,7 +177,7 @@ void WorldSimApi::simPlotLineStrip(const std::vector& points, const ve } // plot line for points 0-1, 2-3, 4-5... must be even number of points -void WorldSimApi::simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +void WorldSimApi::simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) { if (points.size() % 2) { @@ -191,7 +191,7 @@ void WorldSimApi::simPlotLineList(const std::vector& points, const vec } } -void WorldSimApi::simPlotArrowList(const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) +void WorldSimApi::simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) { // assert points_start.size() == poinst_end.size() FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; @@ -201,24 +201,34 @@ void WorldSimApi::simPlotArrowList(const std::vector& points_start, co } } -void WorldSimApi::simPlotTransform(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) +void WorldSimApi::simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) +{ + // assert positions.size() == strings.size() + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (int idx = 0; idx < positions.size(); idx += 1) + { + DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(positions[idx]), FString(strings[idx].c_str()), NULL, color.ToFColor(true), duration, false, scale); + } +} + +void WorldSimApi::simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) { for (const auto& pose : poses) { DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).Rotator(), scale, is_persistent, duration, 0, thickness); } } -// void WorldSimApi::simPlotTransformAndName(const std::vector& poses, const std::vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent); -void WorldSimApi::simPlotStrings(const std::vector& positions, const std::vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) + +void WorldSimApi::simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) { - // assert positions.size() == strings.size() - FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; - for (int idx = 0; idx < positions.size(); idx += 1) + // assert poses.size() == names.size() + FLinearColor color {text_color_rgba[0], text_color_rgba[1], text_color_rgba[2], text_color_rgba[3]}; + for (int idx = 0; idx < poses.size(); idx += 1) { - DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(positions[idx]), FString(strings[idx].c_str()), NULL, color.ToFColor(true), duration, false, scale); + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).Rotator(), tf_scale, false, duration, 0, tf_thickness); + DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), FString(names[idx].c_str()), NULL, color.ToFColor(true), duration, false, text_scale); } } -// void WorldSimApi::simPlotStringOnActor(const std::vector& pose, const std::vector& strings, const std::vector& actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent); //------------------------------------------------- Char APIs -----------------------------------------------------------/ diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 2a91f52f37..31e932f378 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -37,14 +37,13 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; //----------- Plotting APIs ----------/ - virtual void simPlotPoints(const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) override; - virtual void simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; - virtual void simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; - virtual void simPlotArrowList(const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override; - virtual void simPlotTransform(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) override; - // virtual void simPlotTransformAndName(const std::vector& poses, const std::vector& names, float tf_scale, float text_scale, const vector& text_color, float duration, bool is_persistent) override; - virtual void simPlotStrings(const std::vector& position, const std::vector& strings, float scale, const vector& color_rgba, float duration, bool is_persistent) override; - // virtual void simPlotStringOnActor(const std::vector& pose, const std::string& strings, std::string actor_name, float scale, const vector& color_rgba, float duration, bool is_persistent) override; + virtual void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) override; + virtual void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override; + virtual void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) override; + virtual void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) override; + virtual void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) override; //----------- APIs to control ACharacter in scene ----------/ virtual void charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) override; From 0de1110d652451887adb049a5ec686f6cc201ef2 Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 12 Nov 2019 14:52:08 -0800 Subject: [PATCH 4/6] [plotting] add simFlushPersistentMarkers() --- AirLib/include/api/RpcLibClientBase.hpp | 1 + AirLib/include/api/WorldSimApiBase.hpp | 1 + AirLib/src/api/RpcLibClientBase.cpp | 5 +++++ AirLib/src/api/RpcLibServerBase.cpp | 3 +++ PythonClient/airsim/client.py | 6 ++++++ PythonClient/computer_vision/plot_markers.py | 11 +++++++++-- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 5 +++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 1 + 8 files changed, 31 insertions(+), 2 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 8d71d922de..f408839423 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -60,6 +60,7 @@ class RpcLibClientBase { int simGetSegmentationObjectID(const std::string& mesh_name) const; void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0); + void simFlushPersistentMarkers(); void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent); void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 74d99de0f8..f77ddbe3c9 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -43,6 +43,7 @@ class WorldSimApiBase { const std::string& message_param = "", unsigned char severity = 0) = 0; //----------- Plotting APIs ----------/ + virtual void simFlushPersistentMarkers() = 0; virtual void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) = 0; virtual void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; virtual void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index d055fc2900..0a757c38f0 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -242,6 +242,11 @@ void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::strin pimpl_->client.call("simPrintLogMessage", message, message_param, severity); } +void RpcLibClientBase::simFlushPersistentMarkers() +{ + pimpl_->client.call("simFlushPersistentMarkers"); +} + void RpcLibClientBase::simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) { vector conv_points; diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 261a985de1..a50c08e276 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -242,6 +242,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return getWorldSimApi()->setObjectPose(object_name, pose.to(), teleport); }); + pimpl_->server.bind("simFlushPersistentMarkers", [&]() -> void { + getWorldSimApi()->simFlushPersistentMarkers(); + }); pimpl_->server.bind("simPlotPoints", [&](const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) -> void { vector conv_points; RpcLibAdapatorsBase::to(points, conv_points); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 2c45c3c36a..ac68c5cbaf 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -169,6 +169,12 @@ def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name) # Plotting APIs + def simFlushPersistentMarkers(self): + """ + Clear any persistent markers - those plotted with setting is_persistent=True in the APIs below + """ + self.client.call('simFlushPersistentMarkers') + def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, duration = -1.0, is_persistent = False): """ Plot a list of 3D points in World NED frame diff --git a/PythonClient/computer_vision/plot_markers.py b/PythonClient/computer_vision/plot_markers.py index e423dcbbd9..573a986e84 100644 --- a/PythonClient/computer_vision/plot_markers.py +++ b/PythonClient/computer_vision/plot_markers.py @@ -40,11 +40,18 @@ client.simPlotLineList(points = [Vector3r(x,y,-7) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5, duration = 30.0, is_persistent = False) # plot transforms +client.simPlotStrings(strings = ["Microsoft AirSim" for i in range(5)], positions = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,-5,5), np.linspace(0,-5,5))], + scale = 1, color_rgba=[1.0, 1.0, 1.0, 1.0], duration = 10.0) + client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,-7), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=np.pi/2)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], scale = 35, thickness = 5, duration = 10.0, is_persistent = False) client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,-5), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=0.0)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], scale = 35, thickness = 5, duration = 10.0, is_persistent = False) -client.simPlotStrings(strings = ["Microsoft AirSim" for i in range(5)], positions = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,-5,5), np.linspace(0,-5,5))], - scale = 1, color_rgba=[1.0, 1.0, 1.0, 1.0], duration = 10.0) \ No newline at end of file +client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,-1), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=0.0)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], + names=["transform_" + str(idx) for idx in range(10)], tf_scale = 45, tf_thickness = 3, text_scale = 2, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) + +time.sleep(20.0) + +client.simFlushPersistentMarkers() \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 4de9471ee7..98e5f684ee 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -157,6 +157,11 @@ std::unique_ptr> WorldSimApi::swapTextures(const std::s return swappedObjectNames; } //----------- Plotting APIs ----------/ +void WorldSimApi::simFlushPersistentMarkers() +{ + FlushPersistentDebugLines(simmode_->GetWorld()); +} + void WorldSimApi::simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) { FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 31e932f378..4d5e3d59d0 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -37,6 +37,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; //----------- Plotting APIs ----------/ + virtual void simFlushPersistentMarkers() override; virtual void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) override; virtual void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; virtual void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; From f660720845797ed51c34bf62639d46f5425ce368 Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 12 Nov 2019 17:08:21 -0800 Subject: [PATCH 5/6] [plotting] fix types in signature, plot transforms in ENU view frame --- Unreal/Plugins/AirSim/Source/NedTransform.cpp | 8 ++++++++ Unreal/Plugins/AirSim/Source/NedTransform.h | 6 ++++-- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 18 +++++++++--------- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 15 ++++++++------- 4 files changed, 29 insertions(+), 18 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.cpp b/Unreal/Plugins/AirSim/Source/NedTransform.cpp index 81eaee2e3c..45bcf1b706 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.cpp +++ b/Unreal/Plugins/AirSim/Source/NedTransform.cpp @@ -82,6 +82,14 @@ FTransform NedTransform::fromGlobalNed(const Pose& pose) const { return FTransform(fromNed(pose.orientation), fromGlobalNed(pose.position)); } +FQuat NedTransform::fromNedtoEnu(const FQuat& q) const +{ + return FQuat(q.W, q.Y, q.X, -q.Z); +} +FTransform NedTransform::fromGlobalNedToUUENU(const Pose& pose) const +{ + return FTransform(fromNedtoEnu(fromNed(pose.orientation)), fromGlobalNed(pose.position)); +} FVector NedTransform::getGlobalOffset() const { diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index 521ae2f3eb..da0952305e 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.h +++ b/Unreal/Plugins/AirSim/Source/NedTransform.h @@ -3,7 +3,6 @@ #include "CoreMinimal.h" #include "Kismet/KismetMathLibrary.h" #include "GameFramework/Actor.h" - #include "common/Common.hpp" /* @@ -38,7 +37,6 @@ class AIRSIM_API NedTransform Pose toLocalNed(const FTransform& pose) const; Pose toGlobalNed(const FTransform& pose) const; - //local NED -> UU FVector fromLocalNed(const Vector3r& position) const; FVector fromGlobalNed(const Vector3r& position) const; @@ -47,6 +45,10 @@ class AIRSIM_API NedTransform FTransform fromLocalNed(const Pose& pose) const; FTransform fromGlobalNed(const Pose& pose) const; + // NED -> ENU. We want to visualize in world ENU (right handed) + FQuat fromNedtoEnu(const FQuat& q) const; + FTransform fromGlobalNedToUUENU(const Pose& pose) const; + FVector getGlobalOffset() const; FVector getLocalOffset() const; FTransform getGlobalTransform() const; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 98e5f684ee..de484a449c 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -162,7 +162,7 @@ void WorldSimApi::simFlushPersistentMarkers() FlushPersistentDebugLines(simmode_->GetWorld()); } -void WorldSimApi::simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) +void WorldSimApi::simPlotPoints(const std::vector& points, const std::vector& color_rgba, float size, float duration, bool is_persistent) { FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; for (const auto& point : points) @@ -172,7 +172,7 @@ void WorldSimApi::simPlotPoints(const vector& points, const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +void WorldSimApi::simPlotLineStrip(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) { FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; for (size_t idx = 0; idx != points.size()-1; idx++) @@ -182,7 +182,7 @@ void WorldSimApi::simPlotLineStrip(const vector& points, const vector< } // plot line for points 0-1, 2-3, 4-5... must be even number of points -void WorldSimApi::simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +void WorldSimApi::simPlotLineList(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) { if (points.size() % 2) { @@ -196,7 +196,7 @@ void WorldSimApi::simPlotLineList(const vector& points, const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) +void WorldSimApi::simPlotArrows(const std::vector& points_start, const std::vector& points_end, const std::vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) { // assert points_start.size() == poinst_end.size() FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; @@ -206,7 +206,7 @@ void WorldSimApi::simPlotArrows(const vector& points_start, const vect } } -void WorldSimApi::simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) +void WorldSimApi::simPlotStrings(const std::vector& strings, const std::vector& positions, float scale, const std::vector& color_rgba, float duration) { // assert positions.size() == strings.size() FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; @@ -216,21 +216,21 @@ void WorldSimApi::simPlotStrings(const vector& strings, const vecto } } -void WorldSimApi::simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) +void WorldSimApi::simPlotTransforms(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) { for (const auto& pose : poses) { - DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).Rotator(), scale, is_persistent, duration, 0, thickness); + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNedToUUENU(pose).Rotator(), scale, is_persistent, duration, 0, thickness); } } -void WorldSimApi::simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) +void WorldSimApi::simPlotTransformsWithNames(const std::vector& poses, const std::vector& names, float tf_scale, float tf_thickness, float text_scale, const std::vector& text_color_rgba, float duration) { // assert poses.size() == names.size() FLinearColor color {text_color_rgba[0], text_color_rgba[1], text_color_rgba[2], text_color_rgba[3]}; for (int idx = 0; idx < poses.size(); idx += 1) { - DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).Rotator(), tf_scale, false, duration, 0, tf_thickness); + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNedToUUENU(poses[idx]).Rotator(), tf_scale, false, duration, 0, tf_thickness); DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), FString(names[idx].c_str()), NULL, color.ToFColor(true), duration, false, text_scale); } } diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 4d5e3d59d0..048133cf7b 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -10,6 +10,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { public: typedef msr::airlib::Pose Pose; + typedef msr::airlib::Vector3r Vector3r; WorldSimApi(ASimModeBase* simmode); virtual ~WorldSimApi() = default; @@ -38,13 +39,13 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { //----------- Plotting APIs ----------/ virtual void simFlushPersistentMarkers() override; - virtual void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) override; - virtual void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; - virtual void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) override; - virtual void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override; - virtual void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) override; - virtual void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) override; - virtual void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) override; + virtual void simPlotPoints(const std::vector& points, const std::vector& color_rgba, float size, float duration, bool is_persistent) override; + virtual void simPlotLineStrip(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotLineList(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotArrows(const std::vector& points_start, const std::vector& points_end, const std::vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override; + virtual void simPlotStrings(const std::vector& strings, const std::vector& positions, float scale, const std::vector& color_rgba, float duration) override; + virtual void simPlotTransforms(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) override; + virtual void simPlotTransformsWithNames(const std::vector& poses, const std::vector& names, float tf_scale, float tf_thickness, float text_scale, const std::vector& text_color_rgba, float duration) override; //----------- APIs to control ACharacter in scene ----------/ virtual void charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) override; From befada3ec70ce50fce27384e3e31dd5e5ceeac9c Mon Sep 17 00:00:00 2001 From: madratman Date: Thu, 26 Mar 2020 15:46:10 -0700 Subject: [PATCH 6/6] [plotting] mods to python demo script, add placeholder FRU->FLU quat convertor --- PythonClient/computer_vision/plot_markers.py | 22 ++++++++++++------- Unreal/Plugins/AirSim/Source/NedTransform.cpp | 9 ++++---- Unreal/Plugins/AirSim/Source/NedTransform.h | 7 +++--- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 4 ++-- 4 files changed, 25 insertions(+), 17 deletions(-) diff --git a/PythonClient/computer_vision/plot_markers.py b/PythonClient/computer_vision/plot_markers.py index 573a986e84..d005ff8338 100644 --- a/PythonClient/computer_vision/plot_markers.py +++ b/PythonClient/computer_vision/plot_markers.py @@ -40,17 +40,23 @@ client.simPlotLineList(points = [Vector3r(x,y,-7) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5, duration = 30.0, is_persistent = False) # plot transforms -client.simPlotStrings(strings = ["Microsoft AirSim" for i in range(5)], positions = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,-5,5), np.linspace(0,-5,5))], - scale = 1, color_rgba=[1.0, 1.0, 1.0, 1.0], duration = 10.0) +client.simPlotStrings(strings = ["Microsoft AirSim" for i in range(5)], positions = [Vector3r(x,y,-1) for x, y in zip(np.linspace(0,5,5), np.linspace(0,0,5))], + scale = 1, color_rgba=[1.0, 1.0, 1.0, 1.0], duration = 1200.0) -client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,-7), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=np.pi/2)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], - scale = 35, thickness = 5, duration = 10.0, is_persistent = False) +# client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=yaw)) for x, y, yaw in zip(np.linspace(0,10,10), np.linspace(0,0,10), np.linspace(0,np.pi,10))], +# scale = 35, thickness = 5, duration = 1200.0, is_persistent = False) -client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,-5), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=0.0)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], - scale = 35, thickness = 5, duration = 10.0, is_persistent = False) +# client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=roll, yaw=0.0)) for x, y, roll in zip(np.linspace(0,10,10), np.linspace(1,1,10), np.linspace(0,np.pi,10))], +# scale = 35, thickness = 5, duration = 1200.0, is_persistent = False) -client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,-1), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=0.0)) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], - names=["transform_" + str(idx) for idx in range(10)], tf_scale = 45, tf_thickness = 3, text_scale = 2, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) +client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=yaw)) for x, y, yaw in zip(np.linspace(0,10,10), np.linspace(0,0,10), np.linspace(0,np.pi,10))], + names=["tf_yaw_" + str(idx) for idx in range(10)], tf_scale = 35, tf_thickness = 5, text_scale = 1, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) + +client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=roll, yaw=0.0)) for x, y, roll in zip(np.linspace(0,10,10), np.linspace(1,1,10), np.linspace(0,np.pi,10))], + names=["tf_roll_" + str(idx) for idx in range(10)], tf_scale = 35, tf_thickness = 5, text_scale = 1, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) + +client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=pitch, roll=0.0, yaw=0.0)) for x, y, pitch in zip(np.linspace(0,10,10), np.linspace(-1,-1,10), np.linspace(0,np.pi,10))], + names=["tf_pitch_" + str(idx) for idx in range(10)], tf_scale = 35, tf_thickness = 5, text_scale = 1, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) time.sleep(20.0) diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.cpp b/Unreal/Plugins/AirSim/Source/NedTransform.cpp index 45bcf1b706..112e54d355 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.cpp +++ b/Unreal/Plugins/AirSim/Source/NedTransform.cpp @@ -82,13 +82,14 @@ FTransform NedTransform::fromGlobalNed(const Pose& pose) const { return FTransform(fromNed(pose.orientation), fromGlobalNed(pose.position)); } -FQuat NedTransform::fromNedtoEnu(const FQuat& q) const +FQuat NedTransform::fromUUtoFLU(const FQuat& q) const { - return FQuat(q.W, q.Y, q.X, -q.Z); + return FQuat(-q.X, q.Y, -q.Z, q.W); } -FTransform NedTransform::fromGlobalNedToUUENU(const Pose& pose) const +// todo unused. need to manually plots tf axes' line in right handed FLU instead of using DrawDebugCoordinateSystem +FQuat NedTransform::fromGlobalNedToFLU(const Quaternionr& q) const { - return FTransform(fromNedtoEnu(fromNed(pose.orientation)), fromGlobalNed(pose.position)); + return fromUUtoFLU(fromNed(q)); } FVector NedTransform::getGlobalOffset() const diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index da0952305e..1c3ebf644e 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.h +++ b/Unreal/Plugins/AirSim/Source/NedTransform.h @@ -45,9 +45,10 @@ class AIRSIM_API NedTransform FTransform fromLocalNed(const Pose& pose) const; FTransform fromGlobalNed(const Pose& pose) const; - // NED -> ENU. We want to visualize in world ENU (right handed) - FQuat fromNedtoEnu(const FQuat& q) const; - FTransform fromGlobalNedToUUENU(const Pose& pose) const; + // UU -> ROS FLU. UU is XYZ:FrontRightUp (left handed); ROS FLU is XYZ:FrontLeftUp (right handed) + // used by simPlotApis: + FQuat fromUUtoFLU(const FQuat& q) const; + FQuat fromGlobalNedToFLU(const Quaternionr& q) const; FVector getGlobalOffset() const; FVector getLocalOffset() const; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index de484a449c..1cb4db9956 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -220,7 +220,7 @@ void WorldSimApi::simPlotTransforms(const std::vector& poses, float scale, { for (const auto& pose : poses) { - DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNedToUUENU(pose).Rotator(), scale, is_persistent, duration, 0, thickness); + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose.position), simmode_->getGlobalNedTransform().fromNed(pose.orientation).Rotator(), scale, is_persistent, duration, 0, thickness); } } @@ -230,7 +230,7 @@ void WorldSimApi::simPlotTransformsWithNames(const std::vector& poses, con FLinearColor color {text_color_rgba[0], text_color_rgba[1], text_color_rgba[2], text_color_rgba[3]}; for (int idx = 0; idx < poses.size(); idx += 1) { - DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNedToUUENU(poses[idx]).Rotator(), tf_scale, false, duration, 0, tf_thickness); + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx].position), simmode_->getGlobalNedTransform().fromNed(poses[idx].orientation).Rotator(), tf_scale, false, duration, 0, tf_thickness); DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), FString(names[idx].c_str()), NULL, color.ToFColor(true), duration, false, text_scale); } }