[Spinal][Servo][WIP] create a new universal interface to control servos

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp

@@ -0,0 +1,63 @@
+/*
+ * servo.cpp
+ *
+ *  Created on: 2024/3/1
+ *      Author: J.Sugihara
+ *
+ */
+
+#include "servo.h"
+
+void Servo::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh, osMutexId* mutex = NULL)
+{
+  nh_ = nh;
+  nh_->subscribe(servo_ctrl_sub_);
+  nh_->subscribe(servo_torque_ctrl_sub_);
+  servo_handler_.init(huart, hi2c, mutex);
+}
+
+void Servo::update()
+{
+  servo_handler_.update();
+}
+
+void Servo::sendData()
+{
+  for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {
+    const ServoData& s = servo_handler_.getServo()[i];
+    if (s.send_data_flag_ != 0) {
+      ServoData data(static_cast<int16_t>(s.getPresentPosition()),
+                     s.present_temp_,
+                     s.moving_,
+                     s.present_current_,
+                     s.hardware_error_status_);
+      // sendMessage(CAN::MESSAGEID_SEND_SERVO_LIST[i], m_slave_id, 8, reinterpret_cast<uint8_t*>(&data), 1);
+    }
+  }
+}
+
+void Servo::servoControlCallback(const spinal::ServoControlCmd& control_msg)
+{
+  if (control_msg.index_length != control_msg.angles_length) return;
+  for (unsigned int i = 0; i < control_msg.index_length; i++) {
+    ServoData& s = servo_handler_.getServo()[control_msg.index[i]];
+    int32_t goal_pos = static_cast<int32_t>(control_msg.angles[i]);
+    s.setGoalPosition(goal_pos);
+    if (! s.torque_enable_) {
+      s.torque_enable_ = true;
+      servo_handler_.setTorque(i);
+    }
+  }
+}
+
+void Servo::servoTorqueControlCallback(const spinal::ServoTorqueCmd& control_msg)
+{
+  if (control_msg.index_length != control_msg.torque_enable_length) return;
+  for (unsigned int i = 0; i < control_msg.index_length; i++) {
+    ServoData& s = servo_handler_.getServo()[control_msg.index[i]];
+    if (! s.torque_enable_) {
+      s.torque_enable_ = (control_msg.torque_enable[i] != 0) ? true : false;
+      servo_handler_.setTorque(i);
+    }
+  }
+}

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h

@@ -0,0 +1,57 @@
+/*
+ * servo.h
+ *
+ *  Created on: 2024/3/1
+ *      Author: J.Sugihara
+ */
+
+#ifndef APPLICATION_SERVO_TEMP_SERVO_H_
+#define APPLICATION_SERVO_TEMP_SERVO_H_
+
+#include "Dynamixel/dynamixel_serial.h"
+#include <algorithm>
+#include <ros.h>
+#include <spinal/ServoControlCmd.h>
+#include <spinal/ServoStates.h>
+#include <spinal/ServoTorqueCmd.h>
+#include <string.h>
+#include <config.h>
+
+class Initializer;
+
+class Servo
+{
+public:
+  Servo():
+    servo_ctrl_sub_("servo/target_states", &Servo::servoControlCallback,this),
+    servo_torque_ctrl_sub_("servo/torque_enable", &Servo::servoTorqueControlCallback,this)
+  {
+  }
+  ~Servo(){}
+
+  void init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh, osMutexId* mutex);
+  void update();
+  void sendData();
+
+private:
+  ros::NodeHandle* nh_;
+  ros::Subscriber<spinal::ServoControlCmd, Servo> servo_ctrl_sub_;
+  ros::Subscriber<spinal::ServoTorqueCmd, Servo> servo_torque_ctrl_sub_;
+  struct ServoData{
+    int16_t angle;
+    uint8_t temperature;
+    uint8_t moving;
+    int16_t current;
+    uint8_t error;
+    ServoData(uint16_t angle, uint8_t temperature, uint8_t moving, int16_t current, uint8_t error)
+      :angle(angle), temperature(temperature), moving(moving), current(current), error(error){}
+  };
+  void servoControlCallback(const spinal::ServoControlCmd& control_msg);
+  void servoTorqueControlCallback(const spinal::ServoTorqueCmd& control_msg);
+
+  DynamixelSerial servo_handler_;
+  friend class Initializer;
+};
+
+
+#endif /* APPLICATION_SERVO_SERVO_H_ */