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/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/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index 821b7829..f4d0b77d 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(); @@ -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 diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 96a31a23..4df39954 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,99 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) ); } -bool TurtleBot3::check_device_status() +/** + * @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() { - 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)); - } + // 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; - 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()); - } + RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); + rclcpp::shutdown(); + return; } - if (!calibration_set) { - RCLCPP_ERROR(this->get_logger(), "Failed to set IMU calibration after multiple attempts"); - return false; + // 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++; + } } - - const int8_t NOT_CONNECTED_MOTOR = -1; - - // Add error handling for device status check - try { + + // 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); - - switch (device_status) { - case NOT_CONNECTED_MOTOR: - RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); - break; - - default: - break; + + // 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"); } - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Failed to get device status: %s", e.what()); - return false; + } else { + RCLCPP_WARN(this->get_logger(), "Device did not become ready in time, skipping device status check"); } - - 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() @@ -280,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"); @@ -312,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(); @@ -336,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(