Skip to content

Commit

Permalink
[plotting] fix types in signature, plot transforms in ENU view frame
Browse files Browse the repository at this point in the history
  • Loading branch information
@madratman
madratman committed Nov 13, 2019
1 parent 705a55d commit de1df23
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 18 deletions.
8 changes: 8 additions & 0 deletions Unreal/Plugins/AirSim/Source/NedTransform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
7 changes: 5 additions & 2 deletions Unreal/Plugins/AirSim/Source/NedTransform.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
#include "CoreMinimal.h"
#include "Kismet/KismetMathLibrary.h"
#include "GameFramework/Actor.h"

#include "common/Common.hpp"
#include <math.h>

/*
Note on coordinate system
Expand Down Expand Up @@ -37,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;
Expand All @@ -46,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;
Expand Down
18 changes: 9 additions & 9 deletions Unreal/Plugins/AirSim/Source/WorldSimApi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ void WorldSimApi::simFlushPersistentMarkers()
FlushPersistentDebugLines(simmode_->GetWorld());
}

void WorldSimApi::simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent)
void WorldSimApi::simPlotPoints(const std::vector<Vector3r>& points, const std::vector<float>& 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)
Expand All @@ -134,7 +134,7 @@ void WorldSimApi::simPlotPoints(const vector<Vector3r>& points, const vector<flo
}

// plot line for points 0-1, 1-2, 2-3
void WorldSimApi::simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent)
void WorldSimApi::simPlotLineStrip(const std::vector<Vector3r>& points, const std::vector<float>& 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++)
Expand All @@ -144,7 +144,7 @@ void WorldSimApi::simPlotLineStrip(const vector<Vector3r>& points, const vector<
}

// plot line for points 0-1, 2-3, 4-5... must be even number of points
void WorldSimApi::simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent)
void WorldSimApi::simPlotLineList(const std::vector<Vector3r>& points, const std::vector<float>& color_rgba, float thickness, float duration, bool is_persistent)
{
if (points.size() % 2)
{
Expand All @@ -158,7 +158,7 @@ void WorldSimApi::simPlotLineList(const vector<Vector3r>& points, const vector<f
}
}

void WorldSimApi::simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent)
void WorldSimApi::simPlotArrows(const std::vector<Vector3r>& points_start, const std::vector<Vector3r>& points_end, const std::vector<float>& 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]};
Expand All @@ -168,7 +168,7 @@ void WorldSimApi::simPlotArrows(const vector<Vector3r>& points_start, const vect
}
}

void WorldSimApi::simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration)
void WorldSimApi::simPlotStrings(const std::vector<std::string>& strings, const std::vector<Vector3r>& positions, float scale, const std::vector<float>& color_rgba, float duration)
{
// assert positions.size() == strings.size()
FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]};
Expand All @@ -178,21 +178,21 @@ void WorldSimApi::simPlotStrings(const vector<std::string>& strings, const vecto
}
}

void WorldSimApi::simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent)
void WorldSimApi::simPlotTransforms(const std::vector<Pose>& 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<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration)
void WorldSimApi::simPlotTransformsWithNames(const std::vector<Pose>& poses, const std::vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const std::vector<float>& 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);
}
}
Expand Down
15 changes: 8 additions & 7 deletions Unreal/Plugins/AirSim/Source/WorldSimApi.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -37,13 +38,13 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase {

//----------- Plotting APIs ----------/
virtual void simFlushPersistentMarkers() override;
virtual void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) override;
virtual void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) override;
virtual void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) override;
virtual void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override;
virtual void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration) override;
virtual void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) override;
virtual void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration) override;
virtual void simPlotPoints(const std::vector<Vector3r>& points, const std::vector<float>& color_rgba, float size, float duration, bool is_persistent) override;
virtual void simPlotLineStrip(const std::vector<Vector3r>& points, const std::vector<float>& color_rgba, float thickness, float duration, bool is_persistent) override;
virtual void simPlotLineList(const std::vector<Vector3r>& points, const std::vector<float>& color_rgba, float thickness, float duration, bool is_persistent) override;
virtual void simPlotArrows(const std::vector<Vector3r>& points_start, const std::vector<Vector3r>& points_end, const std::vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override;
virtual void simPlotStrings(const std::vector<std::string>& strings, const std::vector<Vector3r>& positions, float scale, const std::vector<float>& color_rgba, float duration) override;
virtual void simPlotTransforms(const std::vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) override;
virtual void simPlotTransformsWithNames(const std::vector<Pose>& poses, const std::vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const std::vector<float>& 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;
Expand Down

0 comments on commit de1df23

Please sign in to comment.