From 28a48c04c6dbdf271a41841157ed15792ead2f7e Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 14:42:29 -0700 Subject: [PATCH 01/12] Remove error handling and logging from last few commits to declutter --- .../include/turtlebot3_node/turtlebot3.hpp | 2 +- turtlebot3_node/src/turtlebot3.cpp | 138 ++++-------------- 2 files changed, 33 insertions(+), 107 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index 821b7829..425904f0 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -79,7 +79,7 @@ class TurtleBot3 : public rclcpp::Node private: void init_dynamixel_sdk_wrapper(const std::string & usb_port); - bool check_device_status(); + void check_device_status(); void add_sensors(); void add_devices(); diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 96a31a23..d10705a3 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -18,7 +18,6 @@ #include #include -#include using robotis::turtlebot3::TurtleBot3; using namespace std::chrono_literals; @@ -29,28 +28,8 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) RCLCPP_INFO(get_logger(), "Init TurtleBot3 Node Main"); node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); - bool initialization_successful = true; - - try { init_dynamixel_sdk_wrapper(usb_port); - - // Check device status with error handling but without exceptions - if (!check_device_status()) { - RCLCPP_ERROR(this->get_logger(), "Device initialization failed, shutting down"); - initialization_successful = false; - return; // Exit constructor early - } - - // Add stabilization delay after calibration - RCLCPP_INFO(this->get_logger(), "Waiting for IMU to stabilize after calibration..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - - // Verify device connectivity before proceeding - if (!dxl_sdk_wrapper_->is_connected_to_device()) { - RCLCPP_ERROR(this->get_logger(), "Lost connection to device after calibration"); - initialization_successful = false; - return; // Exit constructor early - } + check_device_status(); add_motors(); add_wheels(); @@ -58,24 +37,6 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) add_devices(); run(); - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Critical exception in TurtleBot3 initialization: %s", e.what()); - initialization_successful = false; - } catch (...) { - RCLCPP_ERROR(this->get_logger(), "Unknown critical exception in TurtleBot3 initialization"); - initialization_successful = false; - } - - // Handle shutdown outside of the try-catch if initialization failed - if (!initialization_successful) { - RCLCPP_ERROR(this->get_logger(), "Initialization failed, requesting shutdown"); - // Schedule shutdown instead of doing it immediately - auto shutdown_timer = this->create_wall_timer( - std::chrono::milliseconds(100), - []() { - rclcpp::shutdown(); - }); - } } TurtleBot3::Wheels * TurtleBot3::get_wheels() @@ -95,6 +56,7 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) this->declare_parameter("opencr.id"); this->declare_parameter("opencr.baud_rate"); this->declare_parameter("opencr.protocol_version"); + this->declare_parameter("namespace"); this->get_parameter_or("opencr.id", opencr.id, 200); this->get_parameter_or("opencr.baud_rate", opencr.baud_rate, 1000000); @@ -119,95 +81,59 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) ); } -bool TurtleBot3::check_device_status() +void TurtleBot3::check_device_status() { - if (!dxl_sdk_wrapper_->is_connected_to_device()) { - RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); - return false; - } - - std::string sdk_msg; - uint8_t reset = 1; - - // Add retry logic for setting calibration data with longer delays - bool calibration_set = false; - for (int retry = 0; retry < 3 && !calibration_set; retry++) { - if (retry > 0) { - RCLCPP_WARN(this->get_logger(), "Retrying IMU calibration setup (attempt %d of 3)", retry + 1); - // Increase delay to 2 seconds between attempts to give more recovery time - rclcpp::sleep_for(std::chrono::seconds(2)); - } + if (dxl_sdk_wrapper_->is_connected_to_device()) { + std::string sdk_msg; + uint8_t reset = 1; - sdk_msg.clear(); - if (dxl_sdk_wrapper_->set_data_to_device( + dxl_sdk_wrapper_->set_data_to_device( extern_control_table.imu_re_calibration.addr, extern_control_table.imu_re_calibration.length, &reset, - &sdk_msg)) { - calibration_set = true; - - // Add a brief pause after successful command before starting calibration process - // This helps ensure the OpenCR has fully processed the command - rclcpp::sleep_for(std::chrono::milliseconds(500)); + &sdk_msg); RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); rclcpp::sleep_for(std::chrono::seconds(5)); RCLCPP_INFO(this->get_logger(), "Calibration End"); } else { - RCLCPP_WARN(this->get_logger(), "Failed to set IMU calibration: %s", sdk_msg.c_str()); - } - } - - if (!calibration_set) { - RCLCPP_ERROR(this->get_logger(), "Failed to set IMU calibration after multiple attempts"); - return false; + RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); + rclcpp::shutdown(); + return; } const int8_t NOT_CONNECTED_MOTOR = -1; - // Add error handling for device status check - try { - int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); - switch (device_status) { - case NOT_CONNECTED_MOTOR: - RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); - break; + switch (device_status) { + case NOT_CONNECTED_MOTOR: + RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); + break; - default: - break; - } - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Failed to get device status: %s", e.what()); - return false; + default: + break; } - - return true; } void TurtleBot3::add_motors() { RCLCPP_INFO(this->get_logger(), "Add Motors"); - try { - this->declare_parameter("motors.profile_acceleration_constant"); - this->declare_parameter("motors.profile_acceleration"); - - this->get_parameter_or( - "motors.profile_acceleration_constant", - motors_.profile_acceleration_constant, - 214.577); - - this->get_parameter_or( - "motors.profile_acceleration", - motors_.profile_acceleration, - 0.0); - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Failed to add motors: %s", e.what()); - throw; - } + this->declare_parameter("motors.profile_acceleration_constant"); + this->declare_parameter("motors.profile_acceleration"); + + this->get_parameter_or( + "motors.profile_acceleration_constant", + motors_.profile_acceleration_constant, + 214.577); + + this->get_parameter_or( + "motors.profile_acceleration", + motors_.profile_acceleration, + 0.0); } void TurtleBot3::add_wheels() From 050835f641c16cc9ce4ff22688d43c9ca3bcd097 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 15:41:11 -0700 Subject: [PATCH 02/12] Address launch file substitution error --- turtlebot3_bringup/launch/robot.launch.py | 5 ++++- turtlebot3_node/src/turtlebot3.cpp | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/turtlebot3_bringup/launch/robot.launch.py b/turtlebot3_bringup/launch/robot.launch.py index fbb819a0..f819cdf7 100644 --- a/turtlebot3_bringup/launch/robot.launch.py +++ b/turtlebot3_bringup/launch/robot.launch.py @@ -96,7 +96,10 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource( - [ThisLaunchFileDir(), '/turtlebot3_state_publisher.launch.py']), + [os.path.join( + get_package_share_directory('turtlebot3_bringup'), + 'launch', + 'turtlebot3_state_publisher.launch.py')]), launch_arguments={'use_sim_time': use_sim_time, 'namespace': namespace}.items(), ), diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d10705a3..ea9f2624 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -107,6 +107,8 @@ void TurtleBot3::check_device_status() int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( extern_control_table.device_status.addr, extern_control_table.device_status.length); + + RCLCPP_INFO(this->get_logger(), "Device Status integer: %d", device_status); switch (device_status) { case NOT_CONNECTED_MOTOR: From e3e7fe7f42b90e260dee33ee810346e7fe038764 Mon Sep 17 00:00:00 2001 From: Travis Date: Mon, 12 May 2025 16:14:48 -0700 Subject: [PATCH 03/12] Sleep 5 seconds after IMU calibration for consistent bringup success --- turtlebot3_node/src/turtlebot3.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index ea9f2624..14012ace 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -92,7 +92,7 @@ void TurtleBot3::check_device_status() extern_control_table.imu_re_calibration.length, &reset, &sdk_msg); - + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); rclcpp::sleep_for(std::chrono::seconds(5)); RCLCPP_INFO(this->get_logger(), "Calibration End"); @@ -104,6 +104,11 @@ void TurtleBot3::check_device_status() const int8_t NOT_CONNECTED_MOTOR = -1; + // Add a sleep to allow device to fully initialize + rclcpp::sleep_for(std::chrono::seconds(5)); + + RCLCPP_INFO(this->get_logger(), "Retrieving device status integer"); + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( extern_control_table.device_status.addr, extern_control_table.device_status.length); From 227e36f1dd28f5d188f595083ceb9fc3407e03da Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 14 May 2025 14:18:14 -0700 Subject: [PATCH 04/12] Test OpenCR heartbeat before reading device status --- turtlebot3_node/src/turtlebot3.cpp | 59 ++++++++++++++++++++++++------ 1 file changed, 48 insertions(+), 11 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 14012ace..160a9bea 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -81,8 +81,20 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) ); } +/** + * @brief Checks the status of connected devices and performs device initialization. + * + * This method verifies the connection to devices, performs IMU gyro calibration, + * and checks the device status. If no connection is established, it logs an error + * and shuts down the ROS node. The method includes a device status check and + * provides a warning if motors are not connected. + * + * @note Includes a 5-second sleep to allow device initialization and calibration. + * @throws Shuts down ROS node if device connection fails. + */ void TurtleBot3::check_device_status() { + // Check if device is connected and perform IMU gyroscope calibration if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; @@ -103,18 +115,43 @@ void TurtleBot3::check_device_status() } const int8_t NOT_CONNECTED_MOTOR = -1; - - // Add a sleep to allow device to fully initialize - rclcpp::sleep_for(std::chrono::seconds(5)); - - RCLCPP_INFO(this->get_logger(), "Retrieving device status integer"); - - int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); + const int8_t TIME_TO_INITIALIZE = 5; // seconds + + // Perform heartbeat check to verify basic device communication + bool device_responding = false; + for (int attempt = 0; attempt < 3 && !device_responding; attempt++) { + uint8_t test_value = 42; // Distinctive test value + std::string sdk_msg; + + if (dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.heartbeat.addr, + extern_control_table.heartbeat.length, + &test_value, + &sdk_msg)) { + RCLCPP_INFO(this->get_logger(), "Device responded to heartbeat: %s", sdk_msg.c_str()); + device_responding = true; + } else { + RCLCPP_WARN(this->get_logger(), "Device failed to respond to heartbeat on attempt %d, retrying in %d seconds", attempt + 1, TIME_TO_INITIALIZE); + rclcpp::sleep_for(std::chrono::seconds(TIME_TO_INITIALIZE)); + } + } + + // Only attempt to read device status if heartbeat succeeded + int8_t device_status = 0; + if (device_responding) { + try { + device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); + } catch (...) { + RCLCPP_WARN(this->get_logger(), "Failed to read device status despite successful heartbeat"); + device_status = 0; // Default to a safe value + } + } else { + RCLCPP_WARN(this->get_logger(), "Skipping device status read due to heartbeat failure"); + device_status = 0; // Default to a safe value + } - RCLCPP_INFO(this->get_logger(), "Device Status integer: %d", device_status); - switch (device_status) { case NOT_CONNECTED_MOTOR: RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); From 888a3a75d5e5a0a43fb22bb6b7e5da76e4be3f55 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 15 May 2025 13:28:44 -0700 Subject: [PATCH 05/12] Retry motor connection check if device status shows motor not connected --- turtlebot3_node/src/turtlebot3.cpp | 56 +++++++----------------------- 1 file changed, 13 insertions(+), 43 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 160a9bea..38e731b2 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -114,51 +114,21 @@ void TurtleBot3::check_device_status() return; } + // Check if motors are connected, retry after longer interval if not const int8_t NOT_CONNECTED_MOTOR = -1; - const int8_t TIME_TO_INITIALIZE = 5; // seconds - - // Perform heartbeat check to verify basic device communication - bool device_responding = false; - for (int attempt = 0; attempt < 3 && !device_responding; attempt++) { - uint8_t test_value = 42; // Distinctive test value - std::string sdk_msg; + int8_t device_status = NOT_CONNECTED_MOTOR; + for (int i = 0; i < 4; i++) { + device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); - if (dxl_sdk_wrapper_->set_data_to_device( - extern_control_table.heartbeat.addr, - extern_control_table.heartbeat.length, - &test_value, - &sdk_msg)) { - RCLCPP_INFO(this->get_logger(), "Device responded to heartbeat: %s", sdk_msg.c_str()); - device_responding = true; - } else { - RCLCPP_WARN(this->get_logger(), "Device failed to respond to heartbeat on attempt %d, retrying in %d seconds", attempt + 1, TIME_TO_INITIALIZE); - rclcpp::sleep_for(std::chrono::seconds(TIME_TO_INITIALIZE)); - } - } - - // Only attempt to read device status if heartbeat succeeded - int8_t device_status = 0; - if (device_responding) { - try { - device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); - } catch (...) { - RCLCPP_WARN(this->get_logger(), "Failed to read device status despite successful heartbeat"); - device_status = 0; // Default to a safe value - } - } else { - RCLCPP_WARN(this->get_logger(), "Skipping device status read due to heartbeat failure"); - device_status = 0; // Default to a safe value - } - - switch (device_status) { - case NOT_CONNECTED_MOTOR: - RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); - break; - - default: - break; + if (device_status == NOT_CONNECTED_MOTOR) { + RCLCPP_INFO(this->get_logger(), "Motors not yet initialized, retrying in %d seconds", i+1); + rclcpp::sleep_for(std::chrono::seconds(i+1)); + } else { + break; + } + RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); } } From d30d71a2dea0c9a7416097e9fdd6e58dec044c90 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 15 May 2025 13:32:37 -0700 Subject: [PATCH 06/12] Move motors warning out of for loop --- turtlebot3_node/src/turtlebot3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 38e731b2..5aef013a 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -128,8 +128,8 @@ void TurtleBot3::check_device_status() } else { break; } - RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); } + RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); } void TurtleBot3::add_motors() From 7f530fb98a61c2b47a789b1e77b60933725f379b Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 15 May 2025 13:55:47 -0700 Subject: [PATCH 07/12] Only show motor warning when motors do not initialize --- turtlebot3_node/src/turtlebot3.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 5aef013a..f16bb036 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -126,10 +126,13 @@ void TurtleBot3::check_device_status() RCLCPP_INFO(this->get_logger(), "Motors not yet initialized, retrying in %d seconds", i+1); rclcpp::sleep_for(std::chrono::seconds(i+1)); } else { + RCLCPP_INFO(this->get_logger(), "Motors successfully initialized"); break; } } - RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); + if (device_status == NOT_CONNECTED_MOTOR) { + RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); + } } void TurtleBot3::add_motors() From 046bffade9bee93c5a4c6470852f5207a235e096 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 20 May 2025 12:27:42 -0700 Subject: [PATCH 08/12] Create and check fw device_ready flag before checking device_status --- .../include/turtlebot3_node/control_table.hpp | 3 +- turtlebot3_node/src/turtlebot3.cpp | 55 +++++++++++++------ 2 files changed, 41 insertions(+), 17 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp index 3155c6e6..53fdb61b 100644 --- a/turtlebot3_node/include/turtlebot3_node/control_table.hpp +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -46,8 +46,9 @@ typedef struct ControlItem baud_rate = {8, EEPROM, 1, READ}; ControlItem millis = {10, RAM, 4, READ}; - ControlItem micros = {14, RAM, 4, READ}; + // ControlItem micros = {14, RAM, 4, READ}; // Does not match firmware + ControlItem device_ready = {17, RAM, 1, READ}; ControlItem device_status = {18, RAM, 1, READ}; ControlItem heartbeat = {19, RAM, 1, READ_WRITE}; diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index f16bb036..d04c7f64 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -114,24 +114,47 @@ void TurtleBot3::check_device_status() return; } - // Check if motors are connected, retry after longer interval if not - const int8_t NOT_CONNECTED_MOTOR = -1; - int8_t device_status = NOT_CONNECTED_MOTOR; - for (int i = 0; i < 4; i++) { - device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); - - if (device_status == NOT_CONNECTED_MOTOR) { - RCLCPP_INFO(this->get_logger(), "Motors not yet initialized, retrying in %d seconds", i+1); - rclcpp::sleep_for(std::chrono::seconds(i+1)); - } else { - RCLCPP_INFO(this->get_logger(), "Motors successfully initialized"); - break; + // Wait for device to be fully ready before checking status + bool device_ready = false; + const int MAX_RETRIES = 4; + int retries = 0; + + while (!device_ready && retries < MAX_RETRIES) { + try { + device_ready = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_ready.addr, + extern_control_table.device_ready.length); + + if (!device_ready) { + RCLCPP_INFO(this->get_logger(), "Waiting %d seconds for device to become ready... (%d/%d)", + retries+1, retries+1, MAX_RETRIES); + rclcpp::sleep_for(std::chrono::seconds(retries+1)); + retries++; } + } catch (...) { + RCLCPP_WARN(this->get_logger(), "Failed to read device_ready flag, retrying in %d seconds... (%d/%d)", + retries+1, retries+1, MAX_RETRIES); + rclcpp::sleep_for(std::chrono::seconds(retries+1)); + retries++; + } } - if (device_status == NOT_CONNECTED_MOTOR) { - RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); + + // Only check device_status if device is ready + if (device_ready) { + RCLCPP_INFO(this->get_logger(), "Device is ready, retrieving device status"); + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); + + // Check if motors are connected, retry after longer interval if not + const int8_t NOT_CONNECTED_MOTOR = -1; + if (device_status == NOT_CONNECTED_MOTOR) { + RCLCPP_INFO(this->get_logger(), "Motors not initialized"); + } else { + RCLCPP_INFO(this->get_logger(), "Motors successfully initialized"); + } + } else { + RCLCPP_WARN(this->get_logger(), "Device did not become ready in time, skipping device status check"); } } From 8cb70655c8d586ed233775cda167456d12dfaaad Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Sat, 31 May 2025 09:27:08 -0700 Subject: [PATCH 09/12] Slap on a hot messy fix to address bringup segfault --- turtlebot3_node/src/turtlebot3.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d04c7f64..d7877037 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -98,6 +98,7 @@ void TurtleBot3::check_device_status() if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; + uint8_t calibration_wait_time = 5; // seconds dxl_sdk_wrapper_->set_data_to_device( extern_control_table.imu_re_calibration.addr, @@ -105,9 +106,10 @@ void TurtleBot3::check_device_status() &reset, &sdk_msg); - RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); - rclcpp::sleep_for(std::chrono::seconds(5)); - RCLCPP_INFO(this->get_logger(), "Calibration End"); + RCLCPP_INFO(this->get_logger(), "Start %d seconds calibration of Gyro ", calibration_wait_time); + rclcpp::sleep_for(std::chrono::seconds(calibration_wait_time)); + RCLCPP_INFO(this->get_logger(), "Calibration End, waiting %d seconds for device to be ready", calibration_wait_time); + rclcpp::sleep_for(std::chrono::seconds(5)); // HOT MESS FIX } else { RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); rclcpp::shutdown(); From f0179c8506b63b963f695eb9e06d5965b89bb301 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Sat, 31 May 2025 09:34:29 -0700 Subject: [PATCH 10/12] Revert "Slap on a hot messy fix to address bringup segfault" This reverts commit 8cb70655c8d586ed233775cda167456d12dfaaad. --- turtlebot3_node/src/turtlebot3.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d7877037..d04c7f64 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -98,7 +98,6 @@ void TurtleBot3::check_device_status() if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; - uint8_t calibration_wait_time = 5; // seconds dxl_sdk_wrapper_->set_data_to_device( extern_control_table.imu_re_calibration.addr, @@ -106,10 +105,9 @@ void TurtleBot3::check_device_status() &reset, &sdk_msg); - RCLCPP_INFO(this->get_logger(), "Start %d seconds calibration of Gyro ", calibration_wait_time); - rclcpp::sleep_for(std::chrono::seconds(calibration_wait_time)); - RCLCPP_INFO(this->get_logger(), "Calibration End, waiting %d seconds for device to be ready", calibration_wait_time); - rclcpp::sleep_for(std::chrono::seconds(5)); // HOT MESS FIX + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(this->get_logger(), "Calibration End"); } else { RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); rclcpp::shutdown(); From 789b9efdc0eac4357a209794480137078d156ebf Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 4 Jun 2025 16:04:03 -0700 Subject: [PATCH 11/12] Separate analog pins update rate from other sensors --- turtlebot3_node/src/turtlebot3.cpp | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d04c7f64..4df39954 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -246,11 +246,11 @@ void TurtleBot3::add_sensors() is_connected_ir, is_connected_sonar)); - sensors_.push_back( - new sensors::AnalogPins( - node_handle_, - "analog_pins")); - + // Create and store pointer to analog pins sensor + analog_pins_sensor_ = new sensors::AnalogPins( + node_handle_, + "analog_pins"); + sensors_.push_back(analog_pins_sensor_); RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); @@ -278,7 +278,8 @@ void TurtleBot3::run() { RCLCPP_INFO(this->get_logger(), "Run!"); - publish_timer(std::chrono::milliseconds(50)); + publish_timer(std::chrono::milliseconds(50)); // 20 Hz for other sensors + analog_pins_timer(std::chrono::milliseconds(250)); // 4 Hz for analog pins heartbeat_timer(std::chrono::milliseconds(100)); parameter_event_callback(); @@ -302,6 +303,20 @@ void TurtleBot3::publish_timer(const std::chrono::milliseconds timeout) ); } +void TurtleBot3::analog_pins_timer(const std::chrono::milliseconds timeout) +{ + analog_pins_timer_ = this->create_wall_timer( + timeout, + [this]() -> void + { + rclcpp::Time now = this->now(); + if (analog_pins_sensor_) { + analog_pins_sensor_->publish(now, dxl_sdk_wrapper_); + } + } + ); +} + void TurtleBot3::heartbeat_timer(const std::chrono::milliseconds timeout) { heartbeat_timer_ = this->create_wall_timer( From 344bd83898f5db84b07c9eef69865370a762f99e Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 4 Jun 2025 16:08:28 -0700 Subject: [PATCH 12/12] Add pointer and timer for analog pins to header file --- turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index 425904f0..f4d0b77d 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -90,6 +90,7 @@ class TurtleBot3 : public rclcpp::Node void publish_timer(const std::chrono::milliseconds timeout); void heartbeat_timer(const std::chrono::milliseconds timeout); + void analog_pins_timer(const std::chrono::milliseconds timeout); void cmd_vel_callback(); void parameter_event_callback(); @@ -108,11 +109,14 @@ class TurtleBot3 : public rclcpp::Node rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::TimerBase::SharedPtr analog_pins_timer_; std::unique_ptr cmd_vel_sub_; rclcpp::AsyncParametersClient::SharedPtr priv_parameters_client_; rclcpp::Subscription::SharedPtr parameter_event_sub_; + + sensors::AnalogPins * analog_pins_sensor_; }; } // namespace turtlebot3 } // namespace robotis