Merge pull request #103 from resibots/pool_fix

Deadlock fix and nicer reset for robot pool

Author: costashatz

src/examples/robot_pool.cpp

@@ -61,14 +61,18 @@ namespace pool {
 void eval_robot(int i)
 {
     auto robot = pool::robot_pool.get_robot();
-    std::cout << "Robot " << i << " created [" << robot->skeleton() << "]" << std::endl;
+    std::cout << "Robot " << i << " got [" << robot->skeleton() << "]" << std::endl;
 
     /// --- some robot_dart code ---
     simulate_robot(robot);
     // --- do something with the result
 
+    std::cout << "End of simulation " << i << std::endl;
+
     // CRITICAL : free your robot !
     pool::robot_pool.free_robot(robot);
+
+    std::cout << "Robot " << i << " freed!" << std::endl;
 }
 
 int main(int argc, char** argv)

src/robot_dart/robot_pool.cpp

@@ -14,7 +14,7 @@ namespace robot_dart {
                 std::cout.flush();
             }
             auto robot = robot_creator();
-            _set_init_pos(robot->skeleton());
+            _reset_robot(robot);
             _skeletons.push_back(robot->skeleton());
         }
         for (size_t i = 0; i < _pool_size; i++)
@@ -26,8 +26,8 @@ namespace robot_dart {
 
     std::shared_ptr<Robot> RobotPool::get_robot(const std::string& name)
     {
-        std::lock_guard<std::mutex> lock(_skeleton_mutex);
         while (true) {
+            std::lock_guard<std::mutex> lock(_skeleton_mutex);
             for (size_t i = 0; i < _pool_size; i++)
                 if (_free[i]) {
                     _free[i] = false;
@@ -41,21 +41,15 @@ namespace robot_dart {
         std::lock_guard<std::mutex> lock(_skeleton_mutex);
         for (size_t i = 0; i < _pool_size; i++) {
             if (_skeletons[i] == robot->skeleton()) {
-                _set_init_pos(_skeletons[i]);
+                _reset_robot(robot);
                 _free[i] = true;
                 break;
             }
         }
     }
 
-    void RobotPool::_set_init_pos(const dart::dynamics::SkeletonPtr& skel)
+    void RobotPool::_reset_robot(const std::shared_ptr<Robot>& robot)
     {
-        skel->resetPositions();
-        skel->resetVelocities();
-        skel->resetAccelerations();
-        skel->clearExternalForces();
-        skel->clearInternalForces();
-        skel->clearConstraintImpulses();
-        skel->resetCommands();
+        robot->reset();
     }
 } // namespace robot_dart

src/robot_dart/robot_pool.hpp

@@ -31,7 +31,7 @@ namespace robot_dart {
         std::mutex _skeleton_mutex;
         std::string _model_filename;
 
-        virtual void _set_init_pos(const dart::dynamics::SkeletonPtr& skel);
+        virtual void _reset_robot(const std::shared_ptr<Robot>& robot);
     };
 } // namespace robot_dart