@@ -117,7 +117,7 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
117
117
int trying_cnt = 60 ;
118
118
int cnt = 0 ;
119
119
120
- if (controller_id_.size () > 0 ) {
120
+ if (controller_id_.size () > 0 ) {
121
121
while (trying_connect) {
122
122
std::vector<uint8_t > id_arr;
123
123
for (auto controller : controller_id_) {
@@ -614,24 +614,25 @@ bool DynamixelHardware::CommReset()
614
614
return false ;
615
615
}
616
616
617
- bool DynamixelHardware::retryWriteItem (uint8_t id, const std::string& item_name, uint32_t value) {
617
+ bool DynamixelHardware::retryWriteItem (uint8_t id, const std::string & item_name, uint32_t value)
618
+ {
618
619
rclcpp::Time start_time = this ->now ();
619
620
rclcpp::Duration error_duration (0 , 0 );
620
-
621
+
621
622
while (true ) {
622
623
if (dxl_comm_->WriteItem (id, item_name, value) == DxlError::OK) {
623
624
RCLCPP_INFO_STREAM (
624
625
logger_,
625
626
" [ID:" << std::to_string (id) << " ] item_name:" << item_name.c_str () << " \t data:" << value);
626
627
return true ;
627
628
}
628
-
629
+
629
630
error_duration = this ->now () - start_time;
630
631
if (error_duration.seconds () * 1000 >= err_timeout_ms_) {
631
632
RCLCPP_ERROR_STREAM (
632
633
logger_,
633
- " [ID:" << std::to_string (id) << " ] Write Item error (Timeout: " <<
634
- error_duration.seconds () * 1000 << " ms/" << err_timeout_ms_ << " ms)" );
634
+ " [ID:" << std::to_string (id) << " ] Write Item error (Timeout: " <<
635
+ error_duration.seconds () * 1000 << " ms/" << err_timeout_ms_ << " ms)" );
635
636
return false ;
636
637
}
637
638
RCLCPP_WARN_STREAM (
@@ -640,14 +641,15 @@ bool DynamixelHardware::retryWriteItem(uint8_t id, const std::string& item_name,
640
641
}
641
642
}
642
643
643
- bool DynamixelHardware::initItems (const std::string& type_filter) {
644
+ bool DynamixelHardware::initItems (const std::string & type_filter)
645
+ {
644
646
RCLCPP_INFO_STREAM (logger_, " $$$$$ Init Items for type: " << type_filter);
645
647
for (const hardware_interface::ComponentInfo & gpio : info_.gpios ) {
646
648
if (gpio.parameters .at (" type" ) != type_filter) {
647
649
continue ;
648
650
}
649
651
uint8_t id = static_cast <uint8_t >(stoi (gpio.parameters .at (" ID" )));
650
-
652
+
651
653
// First write items containing "Limit"
652
654
for (auto it : gpio.parameters ) {
653
655
if (it.first != " ID" && it.first != " type" && it.first .find (" Limit" ) != std::string::npos) {
0 commit comments