From 4e319441dde319bb4fdabfcfacb08abbec842a93 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Wed, 9 Mar 2016 17:54:35 +0900 Subject: [PATCH] - added first bulk read failure protection code --- .../src/robotis_controller/RobotisController.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index edd2269..16351d8 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -52,9 +52,20 @@ void RobotisController::InitSyncWrite() // bulkread twice for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) _it->second->TxRxPacket(); - usleep(50*1000); for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxRxPacket(); + { + int _error_cnt = 0; + int _result = COMM_SUCCESS; + do { + if(++_error_cnt > 10) + { + ROS_ERROR("[RobotisController] bulk read fail!!"); + exit(-1); + } + usleep(10*1000); + _result = _it->second->TxRxPacket(); + } while (_result != COMM_SUCCESS); + } ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting