Publish analog pins

turtlebot3_node/CMakeLists.txt

@@ -50,6 +50,7 @@ add_library(${PROJECT_NAME}_lib
   "src/sensors/imu.cpp"
   "src/sensors/joint_state.cpp"
   "src/sensors/sensor_state.cpp"
+  "src/sensors/analog_pins.cpp"
 )
 
 set(DEPENDENCIES

turtlebot3_node/include/turtlebot3_node/control_table.hpp

@@ -105,6 +105,13 @@ typedef struct
 
   ControlItem profile_acceleration_left = {174, RAM, 4, READ_WRITE};
   ControlItem profile_acceleration_right = {178, RAM, 4, READ_WRITE};
+
+  ControlItem analog_a0 = {350, RAM, 2, READ};
+  ControlItem analog_a1 = {352, RAM, 2, READ};
+  ControlItem analog_a2 = {354, RAM, 2, READ};
+  ControlItem analog_a3 = {356, RAM, 2, READ};
+  ControlItem analog_a4 = {358, RAM, 2, READ};
+  ControlItem analog_a5 = {360, RAM, 2, READ};
 } ControlTable;
 
 const ControlTable extern_control_table;

turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp

@@ -0,0 +1,50 @@
+// Copyright 2025 ROBOTIS CO., LTD.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_
+#define TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/u_int16_multi_array.hpp"
+
+#include "turtlebot3_node/sensors/sensors.hpp"
+
+namespace robotis
+{
+namespace turtlebot3
+{
+namespace sensors
+{
+class AnalogPins : public Sensors
+{
+public:
+  explicit AnalogPins(
+    std::shared_ptr<rclcpp::Node> & nh,
+    const std::string & topic_name);
+
+  void publish(
+    const rclcpp::Time & now,
+    std::shared_ptr<DynamixelSDKWrapper> & dxl_sdk_wrapper) override;
+
+private:
+  rclcpp::Publisher<std_msgs::msg::UInt16MultiArray>::SharedPtr analog_publisher_;
+};
+}  // namespace sensors
+}  // namespace turtlebot3
+}  // namespace robotis
+
+#endif  // TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_

turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp

@@ -43,6 +43,7 @@
 #include "turtlebot3_node/devices/sound.hpp"
 #include "turtlebot3_node/dynamixel_sdk_wrapper.hpp"
 #include "turtlebot3_node/odometry.hpp"
+#include "turtlebot3_node/sensors/analog_pins.hpp"
 #include "turtlebot3_node/sensors/battery_state.hpp"
 #include "turtlebot3_node/sensors/imu.hpp"
 #include "turtlebot3_node/sensors/joint_state.hpp"

turtlebot3_node/src/sensors/analog_pins.cpp

@@ -0,0 +1,85 @@
+// Copyright 2025 ROBOTIS CO., LTD.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "turtlebot3_node/sensors/analog_pins.hpp"
+
+#include <memory>
+#include <string>
+#include <utility>
+
+using robotis::turtlebot3::sensors::AnalogPins;
+
+AnalogPins::AnalogPins(
+  std::shared_ptr<rclcpp::Node> & nh,
+  const std::string & topic_name)
+: Sensors(nh, "")  // Call parent constructor with node handle and empty frame_id
+{
+  auto qos = rclcpp::QoS(rclcpp::KeepLast(10));
+
+  analog_publisher_ = nh->create_publisher<std_msgs::msg::UInt16MultiArray>(topic_name, qos);
+
+  RCLCPP_INFO(nh->get_logger(), "Succeeded to create analog pins publisher");
+}
+
+void AnalogPins::publish(
+  const rclcpp::Time & now,
+  std::shared_ptr<DynamixelSDKWrapper> & dxl_sdk_wrapper)
+{
+  (void)now;  // Mark as unused intentionally to suppress warning
+
+  try {
+    auto analog_msg = std::make_unique<std_msgs::msg::UInt16MultiArray>();
+
+    // Set up dimensions for the message
+    analog_msg->layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
+    analog_msg->layout.dim[0].label = "analog_pins";
+    analog_msg->layout.dim[0].size = 6;
+    analog_msg->layout.dim[0].stride = 6;
+    analog_msg->layout.data_offset = 0;
+
+    // Initialize data array to hold 6 pin values
+    analog_msg->data.resize(6);
+
+    // Read values from all 6 analog pins (A0-A5)
+    analog_msg->data[0] = dxl_sdk_wrapper->get_data_from_device<uint16_t>(
+        extern_control_table.analog_a0.addr,
+        extern_control_table.analog_a0.length);
+    analog_msg->data[1] = dxl_sdk_wrapper->get_data_from_device<uint16_t>(
+        extern_control_table.analog_a1.addr,
+        extern_control_table.analog_a1.length);
+    analog_msg->data[2] = dxl_sdk_wrapper->get_data_from_device<uint16_t>(
+        extern_control_table.analog_a2.addr,
+        extern_control_table.analog_a2.length);
+    analog_msg->data[3] = dxl_sdk_wrapper->get_data_from_device<uint16_t>(
+        extern_control_table.analog_a3.addr,
+        extern_control_table.analog_a3.length);
+    analog_msg->data[4] = dxl_sdk_wrapper->get_data_from_device<uint16_t>(
+        extern_control_table.analog_a4.addr,
+        extern_control_table.analog_a4.length);
+    analog_msg->data[5] = dxl_sdk_wrapper->get_data_from_device<uint16_t>(
+        extern_control_table.analog_a5.addr,
+        extern_control_table.analog_a5.length);
+
+    // Publish the message
+    analog_publisher_->publish(std::move(analog_msg));
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("analog_pins"),
+      "Exception in analog_pins publish: %s", e.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("analog_pins"),
+      "Unknown exception in analog_pins publish");
+  }
+}

turtlebot3_node/src/turtlebot3.cpp

@@ -56,7 +56,6 @@ 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);
@@ -66,11 +65,19 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port)
 
   dxl_sdk_wrapper_ = std::make_shared<DynamixelSDKWrapper>(opencr);
 
+  // Initialize the main control table range (original range)
   dxl_sdk_wrapper_->init_read_memory(
     extern_control_table.millis.addr,
     (extern_control_table.profile_acceleration_right.addr - extern_control_table.millis.addr) +
     extern_control_table.profile_acceleration_right.length
   );
+
+  // Add a separate initialization for the analog pins range
+  dxl_sdk_wrapper_->init_read_memory(
+    extern_control_table.analog_a0.addr,
+    (extern_control_table.analog_a5.addr - extern_control_table.analog_a0.addr) +
+    extern_control_table.analog_a5.length
+  );
 }
 
 void TurtleBot3::check_device_status()
@@ -198,6 +205,14 @@ void TurtleBot3::add_sensors()
       is_connected_ir,
       is_connected_sonar));
 
+  sensors_.push_back(
+    new sensors::AnalogPins(
+      node_handle_,
+      "analog_pins"));
+
+
+  RCLCPP_INFO(this->get_logger(), "Successfully added all sensors");
+
   dxl_sdk_wrapper_->read_data_set();
   sensors_.push_back(
     new sensors::JointState(