- ROS_DEBUG("pos: %.2f %.2f %.2f %.2f", pos[0]*180/M_PI, pos[1]*180/M_PI, pos[2]*180/M_PI, pos[3]*180/M_PI);
+#ifdef READ_EFFORT
+ if (dynamixel_readword(15, XM_PRESENT_CURRENT, ¤t) == 1) {
+ eff[4] = current*2.69e-3;
+ } else {
+ ROS_ERROR("I2C dynamixel id=15 current read error");
+ }
+#endif
+
+ ROS_DEBUG("pos: %.2f %.2f %.2f %.2f %.2f", pos[0]*180/M_PI, pos[1]*180/M_PI, pos[2]*180/M_PI, pos[3]*180/M_PI, pos[4]);
+ ROS_DEBUG("velocity: %.2f %.2f %.2f %.2f %.2f", vel[0]*30/M_PI, vel[1]*30/M_PI, vel[2]*30/M_PI, vel[3]*30/M_PI, vel[4]);
+#ifdef READ_EFFORT
+ ROS_DEBUG("current: %.2f %.2f %.2f %.2f %.2f", eff[0], eff[1], eff[2], eff[3], eff[4]);
+#endif
+ auto t2 = std::chrono::high_resolution_clock::now();
+ int64_t duration = std::chrono::duration_cast<std::chrono::milliseconds>( t2 - t1 ).count();
+ ROS_DEBUG("read duration: %lldms", duration);