Skip to content

Commit 708b190

Browse files
authored
Merge pull request #2304 from madratman/PR/plot_APIs
[plotting] add RViz like plot APIs for debugging
2 parents 5ff2d49 + befada3 commit 708b190

10 files changed

Lines changed: 386 additions & 2 deletions

File tree

AirLib/include/api/RpcLibClientBase.hpp

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,14 @@ class RpcLibClientBase {
6060
int simGetSegmentationObjectID(const std::string& mesh_name) const;
6161
void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
6262

63+
void simFlushPersistentMarkers();
64+
void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent);
65+
void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
66+
void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
67+
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);
68+
void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration);
69+
void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent);
70+
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);
6371

6472
bool armDisarm(bool arm, const std::string& vehicle_name = "");
6573
bool isApiControlEnabled(const std::string& vehicle_name = "") const;

AirLib/include/api/WorldSimApiBase.hpp

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,16 @@ class WorldSimApiBase {
4242
virtual void printLogMessage(const std::string& message,
4343
const std::string& message_param = "", unsigned char severity = 0) = 0;
4444

45+
//----------- Plotting APIs ----------/
46+
virtual void simFlushPersistentMarkers() = 0;
47+
virtual void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) = 0;
48+
virtual void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
49+
virtual void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
50+
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;
51+
virtual void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration) = 0;
52+
virtual void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) = 0;
53+
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;
54+
4555
virtual std::vector<std::string> listSceneObjects(const std::string& name_regex) const = 0;
4656
virtual Pose getObjectPose(const std::string& object_name) const = 0;
4757
virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0;

AirLib/src/api/RpcLibClientBase.cpp

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -242,6 +242,63 @@ void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::strin
242242
pimpl_->client.call("simPrintLogMessage", message, message_param, severity);
243243
}
244244

245+
void RpcLibClientBase::simFlushPersistentMarkers()
246+
{
247+
pimpl_->client.call("simFlushPersistentMarkers");
248+
}
249+
250+
void RpcLibClientBase::simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent)
251+
{
252+
vector<RpcLibAdapatorsBase::Vector3r> conv_points;
253+
RpcLibAdapatorsBase::from(points, conv_points);
254+
pimpl_->client.call("simPlotPoints", conv_points, color_rgba, size, duration, is_persistent);
255+
}
256+
257+
void RpcLibClientBase::simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent)
258+
{
259+
vector<RpcLibAdapatorsBase::Vector3r> conv_points;
260+
RpcLibAdapatorsBase::from(points, conv_points);
261+
pimpl_->client.call("simPlotLineStrip", conv_points, color_rgba, thickness, duration, is_persistent);
262+
}
263+
264+
void RpcLibClientBase::simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent)
265+
{
266+
vector<RpcLibAdapatorsBase::Vector3r> conv_points;
267+
RpcLibAdapatorsBase::from(points, conv_points);
268+
pimpl_->client.call("simPlotLineList", conv_points, color_rgba, thickness, duration, is_persistent);
269+
}
270+
271+
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)
272+
{
273+
vector<RpcLibAdapatorsBase::Vector3r> conv_points_start;
274+
RpcLibAdapatorsBase::from(points_start, conv_points_start);
275+
vector<RpcLibAdapatorsBase::Vector3r> conv_points_end;
276+
RpcLibAdapatorsBase::from(points_end, conv_points_end);
277+
pimpl_->client.call("simPlotArrows", conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent);
278+
}
279+
280+
void RpcLibClientBase::simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration)
281+
{
282+
vector<RpcLibAdapatorsBase::Vector3r> conv_positions;
283+
RpcLibAdapatorsBase::from(positions, conv_positions);
284+
pimpl_->client.call("simPlotStrings", strings, conv_positions, scale, color_rgba, duration);
285+
}
286+
287+
void RpcLibClientBase::simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent)
288+
{
289+
vector<RpcLibAdapatorsBase::Pose> conv_poses;
290+
RpcLibAdapatorsBase::from(poses, conv_poses);
291+
pimpl_->client.call("simPlotTransforms", conv_poses, scale, thickness, duration, is_persistent);
292+
}
293+
294+
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)
295+
{
296+
vector<RpcLibAdapatorsBase::Pose> conv_poses;
297+
RpcLibAdapatorsBase::from(poses, conv_poses);
298+
pimpl_->client.call("simPlotTransformsWithNames", conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration);
299+
300+
}
301+
245302
bool RpcLibClientBase::simIsPaused() const
246303
{
247304
return pimpl_->client.call("simIsPaused").as<bool>();

AirLib/src/api/RpcLibServerBase.cpp

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -242,6 +242,46 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
242242
return getWorldSimApi()->setObjectPose(object_name, pose.to(), teleport);
243243
});
244244

245+
pimpl_->server.bind("simFlushPersistentMarkers", [&]() -> void {
246+
getWorldSimApi()->simFlushPersistentMarkers();
247+
});
248+
pimpl_->server.bind("simPlotPoints", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) -> void {
249+
vector<Vector3r> conv_points;
250+
RpcLibAdapatorsBase::to(points, conv_points);
251+
getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, duration, is_persistent);
252+
});
253+
pimpl_->server.bind("simPlotLineStrip", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) -> void {
254+
vector<Vector3r> conv_points;
255+
RpcLibAdapatorsBase::to(points, conv_points);
256+
getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, duration, is_persistent);
257+
});
258+
pimpl_->server.bind("simPlotLineList", [&](const std::vector<RpcLibAdapatorsBase::Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) -> void {
259+
vector<Vector3r> conv_points;
260+
RpcLibAdapatorsBase::to(points, conv_points);
261+
getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, duration, is_persistent);
262+
});
263+
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 {
264+
vector<Vector3r> conv_points_start;
265+
RpcLibAdapatorsBase::to(points_start, conv_points_start);
266+
vector<Vector3r> conv_points_end;
267+
RpcLibAdapatorsBase::to(points_end, conv_points_end);
268+
getWorldSimApi()->simPlotArrows(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent);
269+
});
270+
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 {
271+
vector<Vector3r> conv_positions;
272+
RpcLibAdapatorsBase::to(positions, conv_positions);
273+
getWorldSimApi()->simPlotStrings(strings, conv_positions, scale, color_rgba, duration);
274+
});
275+
pimpl_->server.bind("simPlotTransforms", [&](const std::vector<RpcLibAdapatorsBase::Pose>& poses, float scale, float thickness, float duration, bool is_persistent) -> void {
276+
vector<Pose> conv_poses;
277+
RpcLibAdapatorsBase::to(poses, conv_poses);
278+
getWorldSimApi()->simPlotTransforms(conv_poses, scale, thickness, duration, is_persistent);
279+
});
280+
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 {
281+
vector<Pose> conv_poses;
282+
RpcLibAdapatorsBase::to(poses, conv_poses);
283+
getWorldSimApi()->simPlotTransformsWithNames(conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration);
284+
});
245285
pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState {
246286
const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics();
247287
return RpcLibAdapatorsBase::KinematicsState(result);

PythonClient/airsim/client.py

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,109 @@ def getLidarData(self, lidar_name = '', vehicle_name = ''):
168168
def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''):
169169
return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name)
170170

171+
# Plotting APIs
172+
def simFlushPersistentMarkers(self):
173+
"""
174+
Clear any persistent markers - those plotted with setting is_persistent=True in the APIs below
175+
"""
176+
self.client.call('simFlushPersistentMarkers')
177+
178+
def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, duration = -1.0, is_persistent = False):
179+
"""
180+
Plot a list of 3D points in World NED frame
181+
182+
Args:
183+
points (list[Vector3r]): List of Vector3r objects
184+
color_rgba (list, optional): desired RGBA values from 0.0 to 1.0
185+
size (float, optional): Size of plotted point
186+
duration (float, optional): Duration (seconds) to plot for
187+
is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time.
188+
"""
189+
self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent)
190+
191+
def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False):
192+
"""
193+
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]
194+
195+
Args:
196+
points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects
197+
color_rgba (list, optional): desired RGBA values from 0.0 to 1.0
198+
thickness (float, optional): Thickness of line
199+
duration (float, optional): Duration (seconds) to plot for
200+
is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time.
201+
"""
202+
self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent)
203+
204+
def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False):
205+
"""
206+
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]
207+
208+
Args:
209+
points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects. Must be even
210+
color_rgba (list, optional): desired RGBA values from 0.0 to 1.0
211+
thickness (float, optional): Thickness of line
212+
duration (float, optional): Duration (seconds) to plot for
213+
is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time.
214+
"""
215+
self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent)
216+
217+
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):
218+
"""
219+
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]
220+
221+
Args:
222+
points_start (list[Vector3r]): List of 3D start positions of arrow start positions, specified as Vector3r objects
223+
points_end (list[Vector3r]): List of 3D end positions of arrow start positions, specified as Vector3r objects
224+
color_rgba (list, optional): desired RGBA values from 0.0 to 1.0
225+
thickness (float, optional): Thickness of line
226+
arrow_size (float, optional): Size of arrow head
227+
duration (float, optional): Duration (seconds) to plot for
228+
is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time.
229+
"""
230+
self.client.call('simPlotArrows', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent)
231+
232+
233+
def simPlotStrings(self, strings, positions, scale = 5, color_rgba=[1.0, 0.0, 0.0, 1.0], duration = -1.0):
234+
"""
235+
Plots a list of strings at desired positions in World NED frame.
236+
237+
Args:
238+
strings (list[String], optional): List of strings to plot
239+
positions (list[Vector3r]): List of positions where the strings should be plotted. Should be in one-to-one correspondence with the strings' list
240+
scale (float, optional): Font scale of transform name
241+
color_rgba (list, optional): desired RGBA values from 0.0 to 1.0
242+
duration (float, optional): Duration (seconds) to plot for
243+
"""
244+
self.client.call('simPlotStrings', strings, positions, scale, color_rgba, duration)
245+
246+
def simPlotTransforms(self, poses, scale = 5.0, thickness = 5.0, duration = -1.0, is_persistent = False):
247+
"""
248+
Plots a list of transforms in World NED frame.
249+
250+
Args:
251+
poses (list[Pose]): List of Pose objects representing the transforms to plot
252+
scale (float, optional): Length of transforms' axes
253+
thickness (float, optional): Thickness of transforms' axes
254+
duration (float, optional): Duration (seconds) to plot for
255+
is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time.
256+
"""
257+
self.client.call('simPlotTransforms', poses, scale, thickness, duration, is_persistent)
258+
259+
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):
260+
"""
261+
Plots a list of transforms with their names in World NED frame.
262+
263+
Args:
264+
poses (list[Pose]): List of Pose objects representing the transforms to plot
265+
names (list[string]): List of strings with one-to-one correspondence to list of poses
266+
tf_scale (float, optional): Length of transforms' axes
267+
tf_thickness (float, optional): Thickness of transforms' axes
268+
text_scale (float, optional): Font scale of transform name
269+
text_color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 for the transform name
270+
duration (float, optional): Duration (seconds) to plot for
271+
"""
272+
self.client.call('simPlotTransformsWithNames', poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration)
273+
171274
#----------- APIs to control ACharacter in scene ----------/
172275
def simCharSetFaceExpression(self, expression_name, value, character_name = ""):
173276
self.client.call('simCharSetFaceExpression', expression_name, value, character_name)

0 commit comments

Comments
 (0)