diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index f0acd387cb..f75cbeabb8 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 lifetime, bool is_persistent); + void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent); + void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent); + void simPlotArrowList(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float lifetime, bool is_persistent); + void simPlotTransform(const vector& poses, float scale, float thickness, float lifetime, bool is_persistent); + // void simPlotTransformAndName(const vector& poses, const vector& names, float tf_scale, float text_scale, const vector& text_color, float lifetime, bool is_persistent); + void simPlotStrings(const vector& positions, const vector& strings, float scale, const vector& color_rgba, float lifetime, bool is_persistent); + // void simPlotStringOnActor(const vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float lifetime, 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 949c5efd4a..8c679e4a50 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 lifetime, bool is_persistent) = 0; + virtual void simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent) = 0; + virtual void simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float lifetime, 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 lifetime, bool is_persistent) = 0; + virtual void simPlotTransform(const std::vector& poses, float scale, float thickness, float lifetime, 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 lifetime, bool is_persistent) = 0; + virtual void simPlotStrings(const std::vector& position, const std::vector& strings, float scale, const vector& color_rgba, float lifetime, 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 lifetime, 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 82ba021c9b..48265c30ca 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 lifetime, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotPoints", conv_points, color_rgba, size, lifetime, is_persistent); +} + +void RpcLibClientBase::simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotLineStrip", conv_points, color_rgba, thickness, lifetime, is_persistent); +} + +void RpcLibClientBase::simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotLineList", conv_points, color_rgba, thickness, lifetime, is_persistent); +} + +void RpcLibClientBase::simPlotArrowList(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float lifetime, 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, lifetime, is_persistent); + +} + +void RpcLibClientBase::simPlotTransform(const vector& poses, float scale, float thickness, float lifetime, bool is_persistent) +{ + vector conv_poses; + RpcLibAdapatorsBase::from(poses, conv_poses); + pimpl_->client.call("simPlotTransform", conv_poses, scale, thickness, lifetime, is_persistent); +} + +// void RpcLibClientBase::simPlotTransformAndNames(const vector& poses, const vector& names, float tf_scale, float text_scale, const vector& text_color, float lifetime, bool is_persistent) +// { + +// } + +void RpcLibClientBase::simPlotStrings(const vector& positions, const vector& strings, float scale, const vector& color_rgba, float lifetime, bool is_persistent) +{ + vector conv_positions; + RpcLibAdapatorsBase::from(positions, conv_positions); + pimpl_->client.call("simPlotStrings", conv_positions, strings, scale, color_rgba, lifetime, is_persistent); +} + +// void RpcLibClientBase::simPlotStringOnActor(const vector& pose, const std::string& strings, const std::string actor_name, float scale, const vector& color_rgba, float lifetime, 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 3146d46acf..c614d1bcce 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -222,6 +222,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 lifetime, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, lifetime, is_persistent); + }); + pimpl_->server.bind("simPlotLineStrip", [&](const std::vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, lifetime, is_persistent); + }); + pimpl_->server.bind("simPlotLineList", [&](const std::vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, lifetime, 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 lifetime, 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, lifetime, is_persistent); + }); + pimpl_->server.bind("simPlotTransform", [&](const std::vector& poses, float scale, float thickness, float lifetime, bool is_persistent) -> void { + vector conv_poses; + RpcLibAdapatorsBase::to(poses, conv_poses); + getWorldSimApi()->simPlotTransform(conv_poses, scale, thickness, lifetime, 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 lifetime, bool is_persistent) -> void { + // vector conv_poses; + // RpcLibAdapatorsBase::to(poses, conv_poses); + // getWorldSimApi()->simPlotTransformAndNames(conv_poses, names, tf_scale, text_scale, text_color, lifetime, is_persistent); + // }); + pimpl_->server.bind("simPlotStrings", [&](const std::vector& positions, const std::vector strings, float scale, const vector& color_rgba, float lifetime, bool is_persistent) -> void { + vector conv_positions; + RpcLibAdapatorsBase::to(positions, conv_positions); + getWorldSimApi()->simPlotStrings(conv_positions, strings, scale, color_rgba, lifetime, 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 lifetime, bool is_persistent) -> void { + // vector conv_positions; + // RpcLibAdapatorsBase::to(positions, conv_positions); + // getWorldSimApi()->simPlotStringOnActor(conv_positions, strings, scale, color_rgba, lifetime, 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/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 1627aaca44..2b3c0a52ca 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -2,6 +2,7 @@ #include "AirBlueprintLib.h" #include "common/common_utils/Utils.hpp" #include "Weather/WeatherLib.h" +#include "DrawDebugHelpers.h" WorldSimApi::WorldSimApi(ASimModeBase* simmode) : simmode_(simmode) @@ -108,6 +109,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)); @@ -116,6 +118,69 @@ void WorldSimApi::setWeatherParameter(WeatherParameter param, float val) UWeatherLib::setWeatherParamScalar(simmode_->GetWorld(), param_e, val); } +//----------- Plotting APIs ----------/ +void WorldSimApi::simPlotPoints(const std::vector& points, const vector& color_rgba, float size, float lifetime, bool is_persistent) +{ + FColor 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, is_persistent, lifetime); + } +} + +// plot line for points 0-1, 1-2, 2-3 +void WorldSimApi::simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent) +{ + FColor 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, is_persistent, lifetime, 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 lifetime, bool is_persistent) +{ + if (points.size() % 2) + { + + } + + FColor 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, is_persistent, lifetime, 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 lifetime, bool is_persistent) +{ + // assert points_start.size() == poinst_end.size() + FColor 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, is_persistent, lifetime, 0, thickness); + } +} + +void WorldSimApi::simPlotTransform(const std::vector& poses, float scale, float thickness, float lifetime, bool is_persistent) +{ + for (const auto& pose : poses) + { + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).GetLocation(), simmode_->getGlobalNedTransform().fromGlobalNed(pose).Rotator(), scale, is_persistent, lifetime, 0, thickness); + } +} +// void WorldSimApi::simPlotTransformAndName(const std::vector& poses, const std::vector& names, float tf_scale, float text_scale, const vector& text_color, float lifetime, bool is_persistent); +void WorldSimApi::simPlotStrings(const std::vector& positions, const std::vector& strings, float scale, const vector& color_rgba, float lifetime, bool is_persistent) +{ + // assert positions.size() == strings.size() + FColor 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, lifetime, 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 lifetime, bool is_persistent); //------------------------------------------------- Char APIs -----------------------------------------------------------/ diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 66a87c5ae5..7320bc1f0a 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -35,6 +35,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 lifetime, bool is_persistent) override; + virtual void simPlotLineStrip(const std::vector& points, const vector& color_rgba, float thickness, float lifetime, bool is_persistent) override; + virtual void simPlotLineList(const std::vector& points, const vector& color_rgba, float thickness, float lifetime, 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 lifetime, bool is_persistent) override; + virtual void simPlotTransform(const std::vector& poses, float scale, float thickness, float lifetime, 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 lifetime, bool is_persistent) override; + virtual void simPlotStrings(const std::vector& position, const std::vector& strings, float scale, const vector& color_rgba, float lifetime, 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 lifetime, 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;