@@ -70,8 +70,10 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
7070 baud_rate_ = info_.hardware_parameters [" baud_rate" ];
7171 try {
7272 err_timeout_ms_ = stod (info_.hardware_parameters [" error_timeout_ms" ]);
73- } catch (const std::exception& e) {
74- RCLCPP_ERROR (logger_, " Failed to parse error_timeout_ms parameter: %s, using default value" , e.what ());
73+ } catch (const std::exception & e) {
74+ RCLCPP_ERROR (
75+ logger_, " Failed to parse error_timeout_ms parameter: %s, using default value" ,
76+ e.what ());
7577 }
7678
7779 RCLCPP_INFO_STREAM (
@@ -405,10 +407,11 @@ hardware_interface::return_type DynamixelHardware::read(
405407 read_error_duration_ = rclcpp::Duration (0 , 0 );
406408 }
407409 read_error_duration_ = read_error_duration_ + period;
408-
410+
409411 RCLCPP_ERROR_STREAM (
410412 logger_,
411- " Dynamixel Read Fail (Duration: " << read_error_duration_.seconds () * 1000 << " ms/" << err_timeout_ms_ << " ms)" );
413+ " Dynamixel Read Fail (Duration: " << read_error_duration_.seconds () * 1000 << " ms/" <<
414+ err_timeout_ms_ << " ms)" );
412415
413416 if (read_error_duration_.seconds () * 1000 >= err_timeout_ms_) {
414417 return hardware_interface::return_type::ERROR;
@@ -470,10 +473,11 @@ hardware_interface::return_type DynamixelHardware::write(
470473 return hardware_interface::return_type::OK;
471474 } else {
472475 write_error_duration_ = write_error_duration_ + period;
473-
476+
474477 RCLCPP_ERROR_STREAM (
475478 logger_,
476- " Dynamixel Write Fail (Duration: " << write_error_duration_.seconds () * 1000 << " ms/" << err_timeout_ms_ << " ms)" );
479+ " Dynamixel Write Fail (Duration: " << write_error_duration_.seconds () * 1000 << " ms/" <<
480+ err_timeout_ms_ << " ms)" );
477481
478482 if (write_error_duration_.seconds () * 1000 >= err_timeout_ms_) {
479483 return hardware_interface::return_type::ERROR;
0 commit comments