Publish analog pins

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(),
         ),

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};
 

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<TwistSubscriber> cmd_vel_sub_;
 
   rclcpp::AsyncParametersClient::SharedPtr priv_parameters_client_;
   rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
+
+  sensors::AnalogPins * analog_pins_sensor_;
 };
 }  // namespace turtlebot3
 }  // namespace robotis

turtlebot3_node/src/turtlebot3.cpp

@@ -18,7 +18,6 @@
 
 #include <memory>
 #include <string>
-#include <stdexcept>
 
 using robotis::turtlebot3::TurtleBot3;
 using namespace std::chrono_literals;
@@ -29,53 +28,15 @@ 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();
     add_sensors();
     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<uint8_t>("opencr.id");
   this->declare_parameter<int>("opencr.baud_rate");
   this->declare_parameter<float>("opencr.protocol_version");
+  this->declare_parameter<std::string>("namespace");
 
   this->get_parameter_or<uint8_t>("opencr.id", opencr.id, 200);
   this->get_parameter_or<int>("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<bool>(
+        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<int8_t>(
       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<float>("motors.profile_acceleration_constant");
-    this->declare_parameter<float>("motors.profile_acceleration");
-
-    this->get_parameter_or<float>(
-      "motors.profile_acceleration_constant",
-      motors_.profile_acceleration_constant,
-      214.577);
-
-    this->get_parameter_or<float>(
-      "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<float>("motors.profile_acceleration_constant");
+  this->declare_parameter<float>("motors.profile_acceleration");
+
+  this->get_parameter_or<float>(
+    "motors.profile_acceleration_constant",
+    motors_.profile_acceleration_constant,
+    214.577);
+
+  this->get_parameter_or<float>(
+    "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(