diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.cpp b/Unreal/Plugins/AirSim/Source/NedTransform.cpp index 9f30f2445b..43b864364f 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.cpp +++ b/Unreal/Plugins/AirSim/Source/NedTransform.cpp @@ -77,6 +77,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 7512a2cc92..42b855ec9f 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" /* @@ -37,7 +36,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; @@ -46,6 +44,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 dd68125775..11ee38ae9c 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -124,7 +124,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) @@ -134,7 +134,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++) @@ -144,7 +144,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) { @@ -158,7 +158,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]}; @@ -168,7 +168,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]}; @@ -178,21 +178,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 ea1caa73ea..2fa015c6ac 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; @@ -37,13 +38,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;