Merge pull request #2304 from madratman/PR/plot_APIs

[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 simFlushPersistentMarkers();
+    void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent);
+    void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
+    void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
+    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);
+    void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration); 
+    void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent);
+    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);
 
     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 simFlushPersistentMarkers() = 0;
+    virtual void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) = 0; 
+    virtual void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0; 
+    virtual void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0; 
+    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) = 0; 
+    virtual void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration) = 0;
+    virtual void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) = 0; 
+    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) = 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,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<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent)
+{
+    vector<RpcLibAdapatorsBase::Vector3r> conv_points;
+    RpcLibAdapatorsBase::from(points, conv_points);
+    pimpl_->client.call("simPlotPoints", conv_points, color_rgba, size, duration, is_persistent);
+}
+
+void RpcLibClientBase::simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent)
+{
+    vector<RpcLibAdapatorsBase::Vector3r> conv_points;
+    RpcLibAdapatorsBase::from(points, conv_points);
+    pimpl_->client.call("simPlotLineStrip", conv_points, color_rgba, thickness, duration, is_persistent);
+}
+
+void RpcLibClientBase::simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent)
+{
+    vector<RpcLibAdapatorsBase::Vector3r> conv_points;
+    RpcLibAdapatorsBase::from(points, conv_points);
+    pimpl_->client.call("simPlotLineList", conv_points, color_rgba, thickness, duration, is_persistent);
+}
+
+void RpcLibClientBase::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)
+{
+    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("simPlotArrows", conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent);
+}
+
+void RpcLibClientBase::simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration)
+{
+    vector<RpcLibAdapatorsBase::Vector3r> conv_positions;
+    RpcLibAdapatorsBase::from(positions, conv_positions);
+    pimpl_->client.call("simPlotStrings", strings, conv_positions, scale, color_rgba, duration);
+}
+
+void RpcLibClientBase::simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent)
+{
+    vector<RpcLibAdapatorsBase::Pose> conv_poses;
+    RpcLibAdapatorsBase::from(poses, conv_poses);
+    pimpl_->client.call("simPlotTransforms", conv_poses, scale, thickness, duration, is_persistent);
+}
+
+void RpcLibClientBase::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)
+{
+    vector<RpcLibAdapatorsBase::Pose> 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<bool>();

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<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) -> void {
+        vector<Vector3r> conv_points;
+        RpcLibAdapatorsBase::to(points, conv_points);
+        getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, duration, is_persistent);
+    });
+    pimpl_->server.bind("simPlotLineStrip", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) -> void {
+        vector<Vector3r> conv_points;
+        RpcLibAdapatorsBase::to(points, conv_points);
+        getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, duration, is_persistent);
+    });
+    pimpl_->server.bind("simPlotLineList", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) -> void {
+        vector<Vector3r> conv_points;
+        RpcLibAdapatorsBase::to(points, conv_points);
+        getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, duration, is_persistent);
+    });
+    pimpl_->server.bind("simPlotArrows", [&](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 duration, 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()->simPlotArrows(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent);
+    });
+    pimpl_->server.bind("simPlotStrings", [&](const std::vector<std::string> strings, const std::vector<RpcLibAdapatorsBase::Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration) -> void {
+        vector<Vector3r> conv_positions;
+        RpcLibAdapatorsBase::to(positions, conv_positions);
+        getWorldSimApi()->simPlotStrings(strings, conv_positions, scale, color_rgba, duration);
+    });
+    pimpl_->server.bind("simPlotTransforms", [&](const std::vector<RpcLibAdapatorsBase::Pose>& poses, float scale, float thickness, float duration, bool is_persistent) -> void {
+        vector<Pose> conv_poses;
+        RpcLibAdapatorsBase::to(poses, conv_poses);
+        getWorldSimApi()->simPlotTransforms(conv_poses, scale, thickness, duration, is_persistent);
+    });
+    pimpl_->server.bind("simPlotTransformsWithNames", [&](const std::vector<RpcLibAdapatorsBase::Pose>& poses, const std::vector<std::string> names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration) -> void {
+        vector<Pose> 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);

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)