diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 54c30d671a..f408839423 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 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); + 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 4276682c03..f77ddbe3c9 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 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; + 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; 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..0a757c38f0 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -242,6 +242,63 @@ 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; + 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::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("simPlotArrows", conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, 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("simPlotTransforms", conv_poses, scale, thickness, duration, 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_poses; + RpcLibAdapatorsBase::from(poses, conv_poses); + pimpl_->client.call("simPlotTransformsWithNames", conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration); + +} + 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..a50c08e276 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -242,6 +242,46 @@ 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); + 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("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()->simPlotArrows(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); + }); + 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(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("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..ac68c5cbaf 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -168,6 +168,109 @@ 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 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 + + Args: + points (list[Vector3r]): List of Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + 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, 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 (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, 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 (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 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] + + 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 (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('simPlotArrows', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) + + + 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 (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('simPlotTransforms', poses, scale, thickness, 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 = ""): 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..d005ff8338 --- /dev/null +++ b/PythonClient/computer_vision/plot_markers.py @@ -0,0 +1,63 @@ +import setup_path +import airsim +from airsim import Vector3r, Quaternionr, Pose +from airsim.utils import to_quaternion +import numpy as np +import time + +client = airsim.VehicleClient() +client.confirmConnection() + +# plot red arrows for 30 seconds +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.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.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.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) + +# plot points +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.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,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,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,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) + +client.simFlushPersistentMarkers() \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.cpp b/Unreal/Plugins/AirSim/Source/NedTransform.cpp index 81eaee2e3c..112e54d355 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.cpp +++ b/Unreal/Plugins/AirSim/Source/NedTransform.cpp @@ -82,6 +82,15 @@ FTransform NedTransform::fromGlobalNed(const Pose& pose) const { return FTransform(fromNed(pose.orientation), fromGlobalNed(pose.position)); } +FQuat NedTransform::fromUUtoFLU(const FQuat& q) const +{ + return FQuat(-q.X, q.Y, -q.Z, q.W); +} +// 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 fromUUtoFLU(fromNed(q)); +} FVector NedTransform::getGlobalOffset() const { diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index 521ae2f3eb..1c3ebf644e 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,11 @@ class AIRSIM_API NedTransform FTransform fromLocalNed(const Pose& pose) const; FTransform fromGlobalNed(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; FTransform getGlobalTransform() const; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 5120219f46..1cb4db9956 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,84 @@ std::unique_ptr> WorldSimApi::swapTextures(const std::s }, true); return swappedObjectNames; } +//----------- Plotting APIs ----------/ +void WorldSimApi::simFlushPersistentMarkers() +{ + FlushPersistentDebugLines(simmode_->GetWorld()); +} + +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) + { + 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 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++) + { + 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 std::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::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]}; + 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::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]}; + 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 std::vector& poses, float scale, float thickness, float duration, bool is_persistent) +{ + for (const auto& pose : poses) + { + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose.position), simmode_->getGlobalNedTransform().fromNed(pose.orientation).Rotator(), scale, is_persistent, duration, 0, thickness); + } +} + +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].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); + } +} //------------------------------------------------- Char APIs -----------------------------------------------------------/ diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 146426e275..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; @@ -36,6 +37,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 simFlushPersistentMarkers() 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; virtual float charGetFaceExpression(const std::string& expression_name, const std::string& character_name) const override;