@@ -840,6 +840,10 @@ std::string Dynamixel::DxlErrorToString(DxlError error_num)
840
840
841
841
DxlError Dynamixel::ReadMultiDxlData (double period_ms)
842
842
{
843
+ if (read_data_list_.empty ()) {
844
+ return DxlError::OK;
845
+ }
846
+
843
847
if (read_type_ == SYNC) {
844
848
return GetDxlValueFromSyncRead (period_ms);
845
849
} else {
@@ -849,6 +853,10 @@ DxlError Dynamixel::ReadMultiDxlData(double period_ms)
849
853
850
854
DxlError Dynamixel::WriteMultiDxlData ()
851
855
{
856
+ if (write_data_list_.empty ()) {
857
+ return DxlError::OK;
858
+ }
859
+
852
860
if (write_type_ == SYNC) {
853
861
return SetDxlValueToSyncWrite ();
854
862
} else {
@@ -1790,10 +1798,6 @@ DxlError Dynamixel::SetSyncWriteHandler(std::vector<uint8_t> id_arr)
1790
1798
}
1791
1799
DxlError Dynamixel::SetDxlValueToSyncWrite ()
1792
1800
{
1793
- if (write_data_list_.size () == 0 ) {
1794
- return DxlError::OK;
1795
- }
1796
-
1797
1801
for (auto it_write_data : write_data_list_) {
1798
1802
uint8_t comm_id = it_write_data.comm_id ;
1799
1803
uint8_t * param_write_value = new uint8_t [indirect_info_write_[comm_id].size ];
@@ -1977,10 +1981,6 @@ DxlError Dynamixel::SetBulkWriteHandler(std::vector<uint8_t> id_arr)
1977
1981
1978
1982
DxlError Dynamixel::SetDxlValueToBulkWrite ()
1979
1983
{
1980
- if (write_data_list_.size () == 0 ) {
1981
- return DxlError::OK;
1982
- }
1983
-
1984
1984
for (auto it_write_data : write_data_list_) {
1985
1985
uint8_t comm_id = it_write_data.comm_id ;
1986
1986
uint8_t * param_write_value;
0 commit comments