Skip to content

Commit 3dae77e

Browse files
Woojin-CriveWoojin-Crive
committed
refactor: Improve formatting
1 parent 06e8de3 commit 3dae77e

File tree

2 files changed

+74
-49
lines changed

2 files changed

+74
-49
lines changed

‎src/dynamixel/dynamixel.cpp‎

Lines changed: 45 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -105,8 +105,9 @@ DxlError Dynamixel::InitTorqueStates(std::vector<uint8_t> id_arr, bool disable_t
105105

106106
if (torque_state_[it_id] == TORQUE_ON) {
107107
if (disable_torque) {
108-
fprintf(stderr, "[InitTorqueStates][ID:%03d] Torque is enabled, disabling torque\n",
109-
it_id);
108+
fprintf(
109+
stderr, "[InitTorqueStates][ID:%03d] Torque is enabled, disabling torque\n",
110+
it_id);
110111
result = WriteItem(it_id, "Torque Enable", TORQUE_OFF);
111112
if (result != DxlError::OK) {
112113
fprintf(stderr, "[InitTorqueStates][ID:%03d] Error disabling torque\n", it_id);
@@ -115,21 +116,24 @@ DxlError Dynamixel::InitTorqueStates(std::vector<uint8_t> id_arr, bool disable_t
115116
torque_state_[it_id] = TORQUE_OFF;
116117
} else {
117118
torque_state_[it_id] = TORQUE_OFF;
118-
fprintf(stderr,
119-
"[InitTorqueStates][ID:%03d] Torque is enabled, cannot proceed. Set "
120-
"'disable_torque_at_init' parameter to 'true' to disable torque at initialization "
121-
"or disable torque manually.\n",
122-
it_id);
119+
fprintf(
120+
stderr,
121+
"[InitTorqueStates][ID:%03d] Torque is enabled, cannot proceed. Set "
122+
"'disable_torque_at_init' parameter to 'true' to disable torque at initialization "
123+
"or disable torque manually.\n",
124+
it_id);
123125
return DxlError::DLX_HARDWARE_ERROR;
124126
}
125127
}
126128

127-
fprintf(stderr, "[InitTorqueStates][ID:%03d] Current torque state: %s\n", it_id,
128-
torque_state_[it_id] ? "ON" : "OFF");
129+
fprintf(
130+
stderr, "[InitTorqueStates][ID:%03d] Current torque state: %s\n", it_id,
131+
torque_state_[it_id] ? "ON" : "OFF");
129132
}
130133
} catch (const std::exception & e) {
131-
fprintf(stderr, "[InitTorqueStates][ID:%03d] Error checking control item: %s\n", it_id,
132-
e.what());
134+
fprintf(
135+
stderr, "[InitTorqueStates][ID:%03d] Error checking control item: %s\n", it_id,
136+
e.what());
133137
return DxlError::CANNOT_FIND_CONTROL_ITEM;
134138
}
135139
}
@@ -171,7 +175,7 @@ DxlError Dynamixel::InitDxlComm(
171175
} else if (dxl_error != 0) {
172176
fprintf(stderr, " - RX_PACKET_ERROR : %s\n", packet_handler_->getRxPacketError(dxl_error));
173177
uint32_t err = 0;
174-
if(ReadItem(it_id, "Hardware Error Status", err) == DxlError::OK) {
178+
if (ReadItem(it_id, "Hardware Error Status", err) == DxlError::OK) {
175179
fprintf(stderr, "[ID:%03d] Read Hardware Error Status : %x\n", it_id, err);
176180
}
177181
fprintf(stderr, "[ID:%03d] Hardware Error detected, rebooting...\n", it_id);
@@ -266,14 +270,18 @@ DxlError Dynamixel::SetDxlReadItems(
266270
if (existing_item.comm_id == comm_id) {
267271
// Found existing entry, append new items
268272
existing_item.id_arr.insert(existing_item.id_arr.end(), item_ids.begin(), item_ids.end());
269-
existing_item.item_name.insert(existing_item.item_name.end(), item_names.begin(),
270-
item_names.end());
271-
existing_item.item_addr.insert(existing_item.item_addr.end(), item_addrs.begin(),
272-
item_addrs.end());
273-
existing_item.item_size.insert(existing_item.item_size.end(), item_sizes.begin(),
274-
item_sizes.end());
275-
existing_item.item_data_ptr_vec.insert(existing_item.item_data_ptr_vec.end(),
276-
data_vec_ptr.begin(), data_vec_ptr.end());
273+
existing_item.item_name.insert(
274+
existing_item.item_name.end(), item_names.begin(),
275+
item_names.end());
276+
existing_item.item_addr.insert(
277+
existing_item.item_addr.end(), item_addrs.begin(),
278+
item_addrs.end());
279+
existing_item.item_size.insert(
280+
existing_item.item_size.end(), item_sizes.begin(),
281+
item_sizes.end());
282+
existing_item.item_data_ptr_vec.insert(
283+
existing_item.item_data_ptr_vec.end(),
284+
data_vec_ptr.begin(), data_vec_ptr.end());
277285
return DxlError::OK;
278286
}
279287
}
@@ -355,8 +363,9 @@ DxlError Dynamixel::SetDxlWriteItems(
355363
uint16_t ITEM_ADDR;
356364
uint8_t ITEM_SIZE;
357365
if (dxl_info_.GetDxlControlItem(id, it_name, ITEM_ADDR, ITEM_SIZE) == false) {
358-
fprintf(stderr, "[ID:%03d] Cannot find control item in model file : %s\n", id,
359-
it_name.c_str());
366+
fprintf(
367+
stderr, "[ID:%03d] Cannot find control item in model file : %s\n", id,
368+
it_name.c_str());
360369
return DxlError::CANNOT_FIND_CONTROL_ITEM;
361370
}
362371
item_ids.push_back(id);
@@ -367,14 +376,18 @@ DxlError Dynamixel::SetDxlWriteItems(
367376
for (auto & existing_item : write_data_list_) {
368377
if (existing_item.comm_id == comm_id) {
369378
existing_item.id_arr.insert(existing_item.id_arr.end(), item_ids.begin(), item_ids.end());
370-
existing_item.item_name.insert(existing_item.item_name.end(), item_names.begin(),
371-
item_names.end());
372-
existing_item.item_addr.insert(existing_item.item_addr.end(), item_addrs.begin(),
373-
item_addrs.end());
374-
existing_item.item_size.insert(existing_item.item_size.end(), item_sizes.begin(),
375-
item_sizes.end());
376-
existing_item.item_data_ptr_vec.insert(existing_item.item_data_ptr_vec.end(),
377-
data_vec_ptr.begin(), data_vec_ptr.end());
379+
existing_item.item_name.insert(
380+
existing_item.item_name.end(), item_names.begin(),
381+
item_names.end());
382+
existing_item.item_addr.insert(
383+
existing_item.item_addr.end(), item_addrs.begin(),
384+
item_addrs.end());
385+
existing_item.item_size.insert(
386+
existing_item.item_size.end(), item_sizes.begin(),
387+
item_sizes.end());
388+
existing_item.item_data_ptr_vec.insert(
389+
existing_item.item_data_ptr_vec.end(),
390+
data_vec_ptr.begin(), data_vec_ptr.end());
378391
return DxlError::OK;
379392
}
380393
}
@@ -970,8 +983,8 @@ DxlError Dynamixel::SetSyncReadItemAndHandler()
970983
} else {
971984
fprintf(
972985
stderr,
973-
"[ID:%03d] Failed to Set Indirect Address Read Item: [%s], Addr: %d, Size: %d, "
974-
"Error code: %d\n",
986+
"[ID:%03d] Failed to Set Indirect Address Read Item: [%s], Addr: %d, Size: %d, "
987+
"Error code: %d\n",
975988
it_read_data.comm_id,
976989
it_read_data.item_name.at(item_index).c_str(),
977990
it_read_data.item_addr.at(item_index),

‎src/dynamixel_hardware_interface.cpp‎

Lines changed: 29 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -83,13 +83,15 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
8383
if (info_.hardware_parameters.find("disable_torque_at_init") != info_.hardware_parameters.end()) {
8484
disable_torque_at_init = info_.hardware_parameters.at("disable_torque_at_init") == "true";
8585
if (disable_torque_at_init) {
86-
RCLCPP_INFO(logger_,
87-
"Torque will be disabled during initialization if it is enabled at initialization.");
86+
RCLCPP_INFO(
87+
logger_,
88+
"Torque will be disabled during initialization if it is enabled at initialization.");
8889
}
8990
} else {
90-
RCLCPP_INFO(logger_,
91-
"If there is a torque enabled Dynamixel, the program will be terminated. Set "
92-
"'disable_torque_at_init' parameter to 'true' to disable torque at initialization.");
91+
RCLCPP_INFO(
92+
logger_,
93+
"If there is a torque enabled Dynamixel, the program will be terminated. Set "
94+
"'disable_torque_at_init' parameter to 'true' to disable torque at initialization.");
9395
}
9496

9597
RCLCPP_INFO_STREAM(
@@ -194,9 +196,13 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
194196

195197
std::vector<uint8_t> dxl_id_with_virtual_dxl;
196198
dxl_id_with_virtual_dxl.insert(dxl_id_with_virtual_dxl.end(), dxl_id_.begin(), dxl_id_.end());
197-
dxl_id_with_virtual_dxl.insert(dxl_id_with_virtual_dxl.end(), virtual_dxl_id_.begin(),
198-
virtual_dxl_id_.end());
199-
if(dxl_comm_->InitTorqueStates(dxl_id_with_virtual_dxl, disable_torque_at_init) != DxlError::OK) {
199+
dxl_id_with_virtual_dxl.insert(
200+
dxl_id_with_virtual_dxl.end(), virtual_dxl_id_.begin(),
201+
virtual_dxl_id_.end());
202+
if (dxl_comm_->InitTorqueStates(
203+
dxl_id_with_virtual_dxl,
204+
disable_torque_at_init) != DxlError::OK)
205+
{
200206
RCLCPP_ERROR_STREAM(logger_, "Error: InitTorqueStates");
201207
return hardware_interface::CallbackReturn::ERROR;
202208
}
@@ -774,10 +780,12 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
774780
for (const auto & param : gpio.parameters) {
775781
const std::string & param_name = param.first;
776782
if (param_name == "Operating Mode") {
777-
RCLCPP_INFO_STREAM(logger_,
778-
"[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" <<
783+
RCLCPP_INFO_STREAM(
784+
logger_,
785+
"[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" <<
779786
param.second);
780-
if (dxl_comm_->WriteItem(id, param_name,
787+
if (dxl_comm_->WriteItem(
788+
id, param_name,
781789
static_cast<uint32_t>(stoi(param.second))) != DxlError::OK)
782790
{
783791
return false;
@@ -795,10 +803,12 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
795803
continue;
796804
}
797805
if (param_name.find("Limit") != std::string::npos) {
798-
RCLCPP_INFO_STREAM(logger_,
799-
"[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" <<
806+
RCLCPP_INFO_STREAM(
807+
logger_,
808+
"[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" <<
800809
param.second);
801-
if (dxl_comm_->WriteItem(id, param_name,
810+
if (dxl_comm_->WriteItem(
811+
id, param_name,
802812
static_cast<uint32_t>(stoi(param.second))) != DxlError::OK)
803813
{
804814
return false;
@@ -816,10 +826,12 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
816826
{
817827
continue;
818828
}
819-
RCLCPP_INFO_STREAM(logger_,
820-
"[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" <<
829+
RCLCPP_INFO_STREAM(
830+
logger_,
831+
"[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" <<
821832
param.second);
822-
if (dxl_comm_->WriteItem(id, param_name,
833+
if (dxl_comm_->WriteItem(
834+
id, param_name,
823835
static_cast<uint32_t>(stoi(param.second))) != DxlError::OK)
824836
{
825837
return false;

0 commit comments

Comments
 (0)