Fixing segfaults when rendering end effector trajectories.

Author: Peter Fankhauser

free_gait_core/include/free_gait_core/executor/BatchExecutor.hpp

@@ -32,7 +32,7 @@ class BatchExecutor
   bool isProcessing();
   void cancelProcessing();
 
-  StateBatch getStateBatch() const;
+  const StateBatch& getStateBatch() const;
 
  private:
   void processInThread();

free_gait_core/src/executor/BatchExecutor.cpp

@@ -60,7 +60,7 @@ void BatchExecutor::cancelProcessing()
   requestForCancelling_ = true;
 }
 
-StateBatch BatchExecutor::getStateBatch() const
+const StateBatch& BatchExecutor::getStateBatch() const
 {
   if (isProcessing_) throw std::runtime_error("Batch executor error: Cannot access state during processing.");
   return stateBatch_;

free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp

@@ -254,17 +254,17 @@ void FreeGaitPreviewDisplay::jumpToTime()
 void FreeGaitPreviewDisplay::newGoalAvailable()
 {
   ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: New preview available.");
+
   // Auto-enable visuals.
+  ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: Drawing visualizations.");
+  visual_->setStateBatch(playback_.getStateBatch());
   if (autoEnableVisualsProperty_->getBool()) {
     setEnabledRobotModel(true);
     visualsTree_->setBool(true);
+  } else {
+    visual_->showEnabled();
   }
 
-  // Visuals.
-  ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: Drawing visualizations.");
-  visual_->setStateBatch(playback_.getStateBatch());
-  visual_->showEnabled();
-
   // Playback.
   ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: Setting up control.");
   playButtonProperty_->setReadOnly(false);

free_gait_rviz_plugin/src/FreeGaitPreviewPlayback.cpp

@@ -146,7 +146,7 @@ void FreeGaitPreviewPlayback::processingCallback(bool success)
   if (!success) return;
   Lock lock(dataMutex_);
   clear();
-  stateBatch_ = batchExecutor_->getStateBatch();
+  stateBatch_ = batchExecutor_->getStateBatch(); // Deep copy.
   stateBatchComputer_.computeEndEffectorTrajectories(stateBatch_);
   time_.fromSec(stateBatch_.getStartTime());
   ROS_DEBUG_STREAM("Resetting time to " << time_ << ".");

free_gait_rviz_plugin/src/FreeGaitPreviewVisual.cpp

@@ -49,6 +49,7 @@ void FreeGaitPreviewVisual::setEnabledModul(Modul modul, bool enable)
 
 void FreeGaitPreviewVisual::showEnabled()
 {
+  ROS_DEBUG("FreeGaitPreviewVisual::showEnabled()");
   for (const auto& modul : enabledModuls_) {
     switch (modul) {
       case Modul::EndEffectorTrajectories:
@@ -75,36 +76,37 @@ void FreeGaitPreviewVisual::hideEnabled()
 
 void FreeGaitPreviewVisual::showEndEffectorTrajectories(const float width, const Ogre::ColourValue& color)
 {
+  ROS_DEBUG("Rendering end effector trajectories.");
   if (!stateBatchPtr_) return;
 
   // Define size.
   const size_t nEndEffectors(stateBatchPtr_->getEndEffectorPositions().size());
   const size_t nStates(stateBatchPtr_->getStates().size());
 
   // Cleanup.
-  // TODO Segfaults. Whats the problem?
-//  if (endEffectorTrajectories_.size() != nEndEffectors) {
-//    endEffectorTrajectories_.clear();
-//    for (size_t i; i < nEndEffectors; ++i) {
-//      endEffectorTrajectories_.push_back(std::unique_ptr<rviz::BillboardLine>(new rviz::BillboardLine(sceneManager_, frameNode_)));
-//    }
-//  }
+  if (endEffectorTrajectories_.size() != nEndEffectors) {
+    endEffectorTrajectories_.clear();
+    for (size_t i = 0; i < nEndEffectors; ++i) {
+      endEffectorTrajectories_.push_back(std::unique_ptr<rviz::BillboardLine>(new rviz::BillboardLine(sceneManager_, frameNode_)));
+    }
+  }
 
-  for (size_t i; i < nEndEffectors; ++i) {
+  for (size_t i = 0; i < nEndEffectors; ++i) {
+    auto& trajectory = endEffectorTrajectories_[i];
     // For each foot trajectory.
-    endEffectorTrajectories_.push_back(std::unique_ptr<rviz::BillboardLine>(new rviz::BillboardLine(sceneManager_, frameNode_)));
-    endEffectorTrajectories_[i]->clear();
-    endEffectorTrajectories_[i]->setLineWidth(width);
-    endEffectorTrajectories_[i]->setColor(color.r, color.g, color.b, color.a);
-    endEffectorTrajectories_[i]->setNumLines(1);
-    endEffectorTrajectories_[i]->setMaxPointsPerLine(nStates);
+    trajectory->clear();
+    trajectory->setLineWidth(width);
+    trajectory->setColor(color.r, color.g, color.b, color.a);
+    trajectory->setNumLines(1);
+    trajectory->setMaxPointsPerLine(nStates);
 
-    free_gait::Position previousPosition;
+    free_gait::Position previousPosition(free_gait::Position::Zero());
     for (const auto& positionElement : stateBatchPtr_->getEndEffectorPositions()[i]) {
-      auto& position = positionElement.second;
-      if ((position - previousPosition).norm() < 0.01) continue;
+      const auto& position = positionElement.second;
+      if ((position - previousPosition).norm() < 0.01) continue; // TODO Set as parameter.
+      previousPosition = position;
       const auto point = Ogre::Vector3(position.x(), position.y(), position.z());
-      endEffectorTrajectories_[i]->addPoint(point);
+      trajectory->addPoint(point);
     }
   }
 }