Skip to content

Commit 8cb7065

Browse files
Slap on a hot messy fix to address bringup segfault
1 parent 046bffa commit 8cb7065

File tree

1 file changed

+5
-3
lines changed

1 file changed

+5
-3
lines changed

turtlebot3_node/src/turtlebot3.cpp

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -98,16 +98,18 @@ void TurtleBot3::check_device_status()
9898
if (dxl_sdk_wrapper_->is_connected_to_device()) {
9999
std::string sdk_msg;
100100
uint8_t reset = 1;
101+
uint8_t calibration_wait_time = 5; // seconds
101102

102103
dxl_sdk_wrapper_->set_data_to_device(
103104
extern_control_table.imu_re_calibration.addr,
104105
extern_control_table.imu_re_calibration.length,
105106
&reset,
106107
&sdk_msg);
107108

108-
RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro");
109-
rclcpp::sleep_for(std::chrono::seconds(5));
110-
RCLCPP_INFO(this->get_logger(), "Calibration End");
109+
RCLCPP_INFO(this->get_logger(), "Start %d seconds calibration of Gyro ", calibration_wait_time);
110+
rclcpp::sleep_for(std::chrono::seconds(calibration_wait_time));
111+
RCLCPP_INFO(this->get_logger(), "Calibration End, waiting %d seconds for device to be ready", calibration_wait_time);
112+
rclcpp::sleep_for(std::chrono::seconds(5)); // HOT MESS FIX
111113
} else {
112114
RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices");
113115
rclcpp::shutdown();

0 commit comments

Comments
 (0)