Merge pull request #39 from leggedrobotics/fix/do_not_display_empty_s…

…tances free_gait_rviz_plugin: Skip stance display if no foot is in ground contact.
leggedrobotics · Aug 3, 2017 · 517eb1c · 517eb1c
2 parents 3a08ee8 + 630436a
commit 517eb1c
Showing 1 changed file with 1 addition and 0 deletions.
diff --git a/free_gait_rviz_plugin/src/FreeGaitPreviewVisual.cpp b/free_gait_rviz_plugin/src/FreeGaitPreviewVisual.cpp
@@ -200,6 +200,7 @@ void FreeGaitPreviewVisual::showStances(const float alpha)
   stancesMarker_.clear();
 
   for (const auto& stance : stateBatchPtr_->getStances()) {
+    if (stance.second.empty()) continue;
     std_msgs::ColorRGBA color;
     getRainbowColor(stateBatchPtr_->getStartTime(), stateBatchPtr_->getEndTime(), stance.first, color);
     color.a = alpha;