Change usleep(0 to ros::Duration().sleep().

diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp
index 095647e..660e713 100644 (file)
--- a/wam_node_sim/src/wam_node_sim.cpp
+++ b/wam_node_sim/src/wam_node_sim.cpp
@@ -34,7 +34,6 @@
  */
 
 
-#include <unistd.h>
 #include <math.h>
 
 #include <boost/thread.hpp> // BarrettHand threading
@@ -212,10 +211,10 @@ template<size_t DOF>
       // Move j3 in order to give room for hand initialization
       jp_type jp_init = wam.getJointPositions();
       jp_init[3] -= 0.35;
-      usleep(500000);
+      ros::Duration(0.5).sleep();
       wam.moveTo(jp_init);
 
-      usleep(500000);
+      ros::Duration(0.5).sleep();
       hand->initialize();
       hand->update();
       
@@ -512,7 +511,7 @@ template<size_t DOF>
   bool WamNode<DOF>::handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res)
   {
     ROS_INFO("Moving BarrettHand Spread: %.3f m/s", req.velocity);
-    usleep(5000);
+    ros::Duration(0.005).sleep();
     hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD);
     return true;
   }
@@ -658,7 +657,6 @@ template<size_t DOF>
 
       bhand_joint_state.header.stamp = ros::Time::now(); // Set the timestamp
       bhand_joint_state_pub.publish(bhand_joint_state); // Publish the BarrettHand joint states
-//      usleep(1.0 / BHAND_PUBLISH_FREQ*1e6); // Sleep according to the specified publishing frequency
       loop.sleep();
     }
   }