[plotting] add RViz like plot APIs for debugging

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<Vector3r>& points, const vector<float>& color_rgba, float size, float lifetime, bool is_persistent);
+ void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent);
+ void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent);
+ void simPlotArrowList(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float lifetime, bool is_persistent);
+ void simPlotTransform(const vector<Pose>& poses, float scale, float thickness, float lifetime, bool is_persistent);
+ // void simPlotTransformAndName(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float text_scale, const vector<float>& text_color, float lifetime, bool is_persistent); 
+ void simPlotStrings(const vector<Vector3r>& positions, const vector<std::string>& strings, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent); 
+ // void simPlotStringOnActor(const vector<Pose>& pose, const std::string<std::string>& strings, const std::string actor_name, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent); 
 
  bool armDisarm(bool arm, const std::string& vehicle_name = "");
  bool isApiControlEnabled(const std::string& vehicle_name = "") const;

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<Vector3r>& points, const vector<float>& color_rgba, float size, float lifetime, bool is_persistent) = 0; 
+ virtual void simPlotLineStrip(const std::vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent) = 0; 
+ virtual void simPlotLineList(const std::vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent) = 0; 
+ virtual void simPlotArrowList(const std::vector<Vector3r>& points_start, const std::vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float lifetime, bool is_persistent) = 0; 
+ virtual void simPlotTransform(const std::vector<Pose>& poses, float scale, float thickness, float lifetime, bool is_persistent) = 0; 
+ // virtual void simPlotTransformAndName(const std::vector<Pose>& poses, const std::vector<std::string>& names, float tf_scale, float text_scale, const vector<float>& text_color, float lifetime, bool is_persistent) = 0; 
+ virtual void simPlotStrings(const std::vector<Vector3r>& position, const std::vector<std::string>& strings, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent) = 0; 
+ // virtual void simPlotStringOnActor(const std::vector<Pose>& pose, const std::string<std::string>& strings, const std::string actor_name, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent) = 0; 
+
  virtual std::vector<std::string> 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;

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<Vector3r>& points, const vector<float>& color_rgba, float size, float lifetime, bool is_persistent)
+{
+ vector<RpcLibAdapatorsBase::Vector3r> conv_points;
+ RpcLibAdapatorsBase::from(points, conv_points);
+ pimpl_->client.call("simPlotPoints", conv_points, color_rgba, size, lifetime, is_persistent);
+}
+
+void RpcLibClientBase::simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent)
+{
+ vector<RpcLibAdapatorsBase::Vector3r> conv_points;
+ RpcLibAdapatorsBase::from(points, conv_points);
+ pimpl_->client.call("simPlotLineStrip", conv_points, color_rgba, thickness, lifetime, is_persistent);
+}
+
+void RpcLibClientBase::simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent)
+{
+ vector<RpcLibAdapatorsBase::Vector3r> conv_points;
+ RpcLibAdapatorsBase::from(points, conv_points);
+ pimpl_->client.call("simPlotLineList", conv_points, color_rgba, thickness, lifetime, is_persistent);
+}
+
+void RpcLibClientBase::simPlotArrowList(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float lifetime, bool is_persistent)
+{
+ vector<RpcLibAdapatorsBase::Vector3r> conv_points_start;
+ RpcLibAdapatorsBase::from(points_start, conv_points_start);
+ vector<RpcLibAdapatorsBase::Vector3r> 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<Pose>& poses, float scale, float thickness, float lifetime, bool is_persistent)
+{
+ vector<RpcLibAdapatorsBase::Pose> conv_poses;
+ RpcLibAdapatorsBase::from(poses, conv_poses);
+ pimpl_->client.call("simPlotTransform", conv_poses, scale, thickness, lifetime, is_persistent);
+}
+
+// void RpcLibClientBase::simPlotTransformAndNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float text_scale, const vector<float>& text_color, float lifetime, bool is_persistent)
+// {
+
+// } 
+
+void RpcLibClientBase::simPlotStrings(const vector<Vector3r>& positions, const vector<std::string>& strings, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent)
+{
+ vector<RpcLibAdapatorsBase::Vector3r> 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>& pose, const std::string<std::string>& strings, const std::string actor_name, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent)
+// {
+
+// }
+
 bool RpcLibClientBase::simIsPaused() const
 {
  return pimpl_->client.call("simIsPaused").as<bool>();

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<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float size, float lifetime, bool is_persistent) -> void {
+ vector<Vector3r> conv_points;
+ RpcLibAdapatorsBase::to(points, conv_points);
+ getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, lifetime, is_persistent);
+ });
+ pimpl_->server.bind("simPlotLineStrip", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent) -> void {
+ vector<Vector3r> conv_points;
+ RpcLibAdapatorsBase::to(points, conv_points);
+ getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, lifetime, is_persistent);
+ });
+ pimpl_->server.bind("simPlotLineList", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent) -> void {
+ vector<Vector3r> conv_points;
+ RpcLibAdapatorsBase::to(points, conv_points);
+ getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, lifetime, is_persistent);
+ });
+ pimpl_->server.bind("simPlotArrowList", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points_start, const std::vector<RpcLibAdapatorsBase::Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float lifetime, bool is_persistent) -> void {
+ vector<Vector3r> conv_points_start;
+ RpcLibAdapatorsBase::to(points_start, conv_points_start);
+ vector<Vector3r> 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<RpcLibAdapatorsBase::Pose>& poses, float scale, float thickness, float lifetime, bool is_persistent) -> void {
+ vector<Pose> conv_poses;
+ RpcLibAdapatorsBase::to(poses, conv_poses);
+ getWorldSimApi()->simPlotTransform(conv_poses, scale, thickness, lifetime, is_persistent);
+ });
+ // pimpl_->server.bind("simPlotTransformAndNames", [&](const std::vector<RpcLibAdapatorsBase::Pose>& poses, const std::vector<std::string> names, float tf_scale, float text_scale, const vector<float>& text_color, float lifetime, bool is_persistent) -> void {
+ // vector<Pose> 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<RpcLibAdapatorsBase::Vector3r>& positions, const std::vector<std::string> strings, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent) -> void {
+ vector<Vector3r> 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<RpcLibAdapatorsBase::Vector3r>& positions, const std::vector<std::string> strings, const std::string actor_name, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent) -> void {
+ // vector<Vector3r> 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);

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<unsigned char>(msr::airlib::Utils::toNumeric<WeatherParameter>(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<Vector3r>& points, const vector<float>& 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<Vector3r>& points, const vector<float>& 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<Vector3r>& points, const vector<float>& 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<Vector3r>& points_start, const std::vector<Vector3r>& points_end, const vector<float>& 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<Pose>& 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<Pose>& poses, const std::vector<std::string>& names, float tf_scale, float text_scale, const vector<float>& text_color, float lifetime, bool is_persistent); 
+void WorldSimApi::simPlotStrings(const std::vector<Vector3r>& positions, const std::vector<std::string>& strings, float scale, const vector<float>& 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>& pose, const std::vector<std::string>& strings, const std::vector<std::string>& actor_name, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent); 
 
 //------------------------------------------------- Char APIs -----------------------------------------------------------/
 

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<Vector3r>& points, const vector<float>& color_rgba, float size, float lifetime, bool is_persistent) override; 
+ virtual void simPlotLineStrip(const std::vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent) override; 
+ virtual void simPlotLineList(const std::vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float lifetime, bool is_persistent) override; 
+ virtual void simPlotArrowList(const std::vector<Vector3r>& points_start, const std::vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float lifetime, bool is_persistent) override; 
+ virtual void simPlotTransform(const std::vector<Pose>& poses, float scale, float thickness, float lifetime, bool is_persistent) override; 
+ // virtual void simPlotTransformAndName(const std::vector<Pose>& poses, const std::vector<std::string>& names, float tf_scale, float text_scale, const vector<float>& text_color, float lifetime, bool is_persistent) override; 
+ virtual void simPlotStrings(const std::vector<Vector3r>& position, const std::vector<std::string>& strings, float scale, const vector<float>& color_rgba, float lifetime, bool is_persistent) override; 
+ // virtual void simPlotStringOnActor(const std::vector<Pose>& pose, const std::string<std::string>& strings, std::string actor_name, float scale, const vector<float>& 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;