Expose driver enumerations in messages

CMakeLists.txt

@@ -11,10 +11,15 @@ find_package(rclcpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ActiveErrors.msg"
+  "msg/AxisState.msg"
   "msg/ControlMessage.msg"
+  "msg/ControlMode.msg"
   "msg/ControllerStatus.msg"
+  "msg/InputMode.msg"
   "msg/ODriveStatus.msg"
-  "srv/AxisState.srv"
+  "msg/ProcedureResult.msg"
+  "srv/RequestAxisState.srv"
 )
 ament_export_dependencies(rosidl_default_runtime)
 
@@ -42,6 +47,8 @@ install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}
 )
 
+install(DIRECTORY include/ DESTINATION include)
+
 rosidl_get_typesupport_target(cpp_typesupport_target
   ${PROJECT_NAME} rosidl_typesupport_cpp)
 

README.md

@@ -84,4 +84,4 @@ In the meantime, here is how you can use the [odrive python package](https://pyp
     ... # Node setup
     ctrl_stat = ControllerStatus()
     ... # receive message data
-    print(ProcedureResult(ctrl_stat.procedure_result))
+    print(ProcedureResult(ctrl_stat.procedure_result))

include/odrive_can_node.hpp

@@ -5,7 +5,7 @@
 #include "odrive_can/msg/o_drive_status.hpp"
 #include "odrive_can/msg/controller_status.hpp"
 #include "odrive_can/msg/control_message.hpp"
-#include "odrive_can/srv/axis_state.hpp"
+#include "odrive_can/srv/request_axis_state.hpp"
 #include "socket_can.hpp"
 
 #include <mutex>
@@ -22,7 +22,7 @@ using ODriveStatus = odrive_can::msg::ODriveStatus;
 using ControllerStatus = odrive_can::msg::ControllerStatus;
 using ControlMessage = odrive_can::msg::ControlMessage;
 
-using AxisState = odrive_can::srv::AxisState;
+using RequestAxisState = odrive_can::srv::RequestAxisState;
 
 class ODriveCanNode : public rclcpp::Node {
 public:
@@ -32,7 +32,7 @@ class ODriveCanNode : public rclcpp::Node {
 private:
     void recv_callback(const can_frame& frame);
     void subscriber_callback(const ControlMessage::SharedPtr msg);
-    void service_callback(const std::shared_ptr<AxisState::Request> request, std::shared_ptr<AxisState::Response> response);
+    void service_callback(const std::shared_ptr<RequestAxisState::Request> request, std::shared_ptr<RequestAxisState::Response> response);
     void request_state_callback();
     void ctrl_msg_callback();
     inline bool verify_length(const std::string&name, uint8_t expected, uint8_t length);
@@ -59,7 +59,7 @@ class ODriveCanNode : public rclcpp::Node {
     uint32_t axis_state_;
     std::mutex axis_state_mutex_;
     std::condition_variable fresh_heartbeat_;
-    rclcpp::Service<AxisState>::SharedPtr service_;
+    rclcpp::Service<RequestAxisState>::SharedPtr service_;
 
 };
 

msg/ActiveErrors.msg

@@ -0,0 +1,26 @@
+# Note that this is a bit-wise accumulation of errors, where each active bit represents one specific error type
+uint32 errors
+
+# Active Error types
+uint32 INITIALIZING = 0x1
+uint32 SYSTEM_LEVEL = 0x2
+uint32 TIMING_ERROR = 0x4
+uint32 MISSING_ESTIMATE = 0x8
+uint32 BAD_CONFIG = 0x10
+uint32 DRV_FAULT = 0x20
+uint32 MISSING_INPUT = 0x40
+uint32 DC_BUS_OVER_VOLTAGE = 0x100
+uint32 DC_BUS_UNDER_VOLTAGE = 0x200
+uint32 DC_BUS_OVER_CURRENT = 0x400
+uint32 DC_BUS_OVER_REGEN_CURRENT = 0x800
+uint32 CURRENT_LIMITATION_VIOLATION = 0x1000
+uint32 MOTOR_OVER_TEMP = 0x2000
+uint32 INVERTER_OVER_TEMP = 0x4000
+uint32 VELOCITY_LIMIT_VIOLATION = 0x8000
+uint32 POSITION_LIMIT_VIOLATION = 0x10000
+uint32 WATCHDOG_TIMER_EXPIRED = 0x1000000
+uint32 ESTOP_REQUESTED = 0x2000000
+uint32 SPINOUT_DETECTED = 0x4000000
+uint32 BRAKE_RESISTOR_DISARMED = 0x8000000
+uint32 THERMISTOR_DISCONNECTED = 0x10000000
+uint32 CALIBRATION_ERROR = 0x40000000

msg/AxisState.msg

@@ -0,0 +1,17 @@
+uint8 state
+
+# Axis State enumerations
+uint8 UNDEFINED = 0x0
+uint8 IDLE = 0x1
+uint8 STARTUP_SEQUENCE = 0x2
+uint8 FULL_CALIBRATION_SEQUENCE = 0x3
+uint8 MOTOR_CALIBRATION = 0x4
+uint8 ENCODER_INDEX_SEARCH = 0x6
+uint8 ENCODER_OFFSET_CALIBRATION = 0x7
+uint8 CLOSED_LOOP_CONTROL = 0x8
+uint8 LOCKIN_SPIN = 0x9
+uint8 ENCODER_DIR_FIND = 0xA
+uint8 HOMING = 0xB
+uint8 ENCODER_HALL_POLARITY_CALIBRATION = 0xC
+uint8 ENCODER_HALL_PHASE_CALIBRATION = 0xD
+uint8 ANTICOGGING_CALIBRATION = 0xE

msg/ControlMessage.msg

@@ -1,5 +1,5 @@
-uint32 control_mode
-uint32 input_mode
+ControlMode control_mode
+InputMode input_mode
 float32 input_pos
 float32 input_vel
 float32 input_torque

msg/ControlMode.msg

@@ -0,0 +1,17 @@
+uint32 mode
+
+# Control Mode enumerations
+# This mode is not used internally.
+uint32 VOLTAGE_CONTROL = 0x0
+
+# Uses only the inner torque control loop.
+# Use input_torque to command desired torque. 
+uint32 TORQUE_CONTROL = 0x1
+
+# Uses both the inner torque control loop and the velocity control loop.
+# Use input_vel to command desired velocity, and input_torque.
+uint32 VELOCITY_CONTROL = 0x2
+
+# Uses the inner torque loop, the velocity control loop, and the outer position control loop.
+# Use input_pos to command desired position, input_vel to command velocity feed-forward, and input_torque for torque feed-forward.
+uint32 POSITION_CONTROL = 0x3

msg/ControllerStatus.msg

@@ -4,7 +4,7 @@ float32 torque_target
 float32 torque_estimate
 float32 iq_setpoint
 float32 iq_measured
-uint32 active_errors
-uint8 axis_state
-uint8 procedure_result
+ActiveErrors active_errors
+AxisState axis_state
+ProcedureResult procedure_result
 bool trajectory_done_flag

msg/InputMode.msg

@@ -0,0 +1,12 @@
+uint32 mode
+
+# Input Mode enumerations
+uint32 INACTIVE = 0x0
+uint32 PASSTHROUGH = 0x1
+uint32 VEL_RAMP = 0x2
+uint32 POS_FILTER = 0x3
+uint32 MIX_CHANNELS = 0x4
+uint32 TRAP_TRAJ = 0x5
+uint32 TORQUE_RAMP = 0x6
+uint32 MIRROR = 0x7
+uint32 TUNING = 0x8

msg/ODriveStatus.msg

@@ -2,5 +2,5 @@ float32 bus_voltage
 float32 bus_current
 float32 fet_temperature
 float32 motor_temperature
-uint32 active_errors
-uint32 disarm_reason
+ActiveErrors active_errors
+ActiveErrors disarm_reason

msg/ProcedureResult.msg

@@ -0,0 +1,19 @@
+uint8 result
+
+# Procedure Result enumerations
+uint8 SUCCESS = 0x0
+uint8 BUSY = 0x1
+uint8 CANCELLED = 0x2
+uint8 DISARMED = 0x3
+uint8 NO_RESPONSE = 0x4
+uint8 POLE_PAIR_CPR_MISMATCH = 0x5
+uint8 PHASE_RESISTANCE_OUT_OF_RANGE = 0x6
+uint8 PHASE_INDUCTANCE_OUT_OF_RANGE = 0x7
+uint8 UNBALANCED_PHASES = 0x8
+uint8 INVALID_MOTOR_TYPE = 0x9
+uint8 ILLEGAL_HALL_STATE = 0xA
+uint8 TIMEOUT = 0xB
+uint8 HOMING_WITHOUT_ENDSTOP = 0xC
+uint8 INVALID_STATE = 0xD
+uint8 NOT_CALIBRATED = 0xE
+uint8 NOT_CONVERGING = 0xF

src/odrive_can_node.cpp

@@ -41,7 +41,7 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n
     subscriber_ = rclcpp::Node::create_subscription<ControlMessage>("control_message", ctrl_msg_qos, std::bind(&ODriveCanNode::subscriber_callback, this, _1));
 
     rclcpp::QoS srv_qos(rclcpp::KeepAll{});
-    service_ = rclcpp::Node::create_service<AxisState>("request_axis_state", std::bind(&ODriveCanNode::service_callback, this, _1, _2), srv_qos);
+    service_ = rclcpp::Node::create_service<RequestAxisState>("request_axis_state", std::bind(&ODriveCanNode::service_callback, this, _1, _2), srv_qos);
 }
 
 void ODriveCanNode::deinit() {
@@ -80,19 +80,19 @@ void ODriveCanNode::recv_callback(const can_frame& frame) {
         case CmdId::kHeartbeat: {
             if (!verify_length("kHeartbeat", 8, frame.len)) break;
             std::lock_guard<std::mutex> guard(ctrl_stat_mutex_);
-            ctrl_stat_.active_errors    = read_le<uint32_t>(frame.data + 0);
-            ctrl_stat_.axis_state        = read_le<uint8_t>(frame.data + 4);
-            ctrl_stat_.procedure_result  = read_le<uint8_t>(frame.data + 5);
-            ctrl_stat_.trajectory_done_flag = read_le<bool>(frame.data + 6);
+            ctrl_stat_.active_errors.errors    = read_le<uint32_t>(frame.data + 0);
+            ctrl_stat_.axis_state.state        = read_le<uint8_t>(frame.data + 4);
+            ctrl_stat_.procedure_result.result = read_le<uint8_t>(frame.data + 5);
+            ctrl_stat_.trajectory_done_flag    = read_le<bool>(frame.data + 6);
             ctrl_pub_flag_ |= 0b0001;
             fresh_heartbeat_.notify_one();
             break;
         }
         case CmdId::kGetError: {
             if (!verify_length("kGetError", 8, frame.len)) break;
             std::lock_guard<std::mutex> guard(odrv_stat_mutex_);
-            odrv_stat_.active_errors = read_le<uint32_t>(frame.data + 0);
-            odrv_stat_.disarm_reason = read_le<uint32_t>(frame.data + 4);
+            odrv_stat_.active_errors.errors = read_le<uint32_t>(frame.data + 0);
+            odrv_stat_.disarm_reason.errors = read_le<uint32_t>(frame.data + 4);
             odrv_pub_flag_ |= 0b001;
             break;
         }
@@ -159,18 +159,18 @@ void ODriveCanNode::subscriber_callback(const ControlMessage::SharedPtr msg) {
     sub_evt_.set();
 }
 
-void ODriveCanNode::service_callback(const std::shared_ptr<AxisState::Request> request, std::shared_ptr<AxisState::Response> response) {
+void ODriveCanNode::service_callback(const std::shared_ptr<RequestAxisState::Request> request, std::shared_ptr<RequestAxisState::Response> response) {
     {
         std::unique_lock<std::mutex> guard(axis_state_mutex_);
-        axis_state_ = request->axis_requested_state;
+        axis_state_ = request->axis_requested_state.state;
         RCLCPP_INFO(rclcpp::Node::get_logger(), "requesting axis state: %d", axis_state_);
     }
     srv_evt_.set();
 
     std::unique_lock<std::mutex> guard(ctrl_stat_mutex_); // define lock for controller status
     auto call_time = std::chrono::steady_clock::now();
     fresh_heartbeat_.wait(guard, [this, &call_time]() {
-        bool complete = (this->ctrl_stat_.procedure_result != 1) && // make sure procedure_result is not busy
+        bool complete = (this->ctrl_stat_.procedure_result.result != 1) && // make sure procedure_result is not busy
             (std::chrono::steady_clock::now() - call_time >= std::chrono::seconds(1)); // wait for minimum one second 
         return complete; 
         }); // wait for procedure_result
@@ -198,9 +198,9 @@ void ODriveCanNode::ctrl_msg_callback() {
     frame.can_id = node_id_ << 5 | kSetControllerMode;
     {
         std::lock_guard<std::mutex> guard(ctrl_msg_mutex_);
-        write_le<uint32_t>(ctrl_msg_.control_mode, frame.data);
-        write_le<uint32_t>(ctrl_msg_.input_mode,   frame.data + 4);
-        control_mode = ctrl_msg_.control_mode;
+        write_le<uint32_t>(ctrl_msg_.control_mode.mode, frame.data);
+        write_le<uint32_t>(ctrl_msg_.input_mode.mode,   frame.data + 4);
+        control_mode = ctrl_msg_.control_mode.mode;
     }
     frame.len = 8;
     can_intf_.send_can_frame(frame);

srv/RequestAxisState.srv

@@ -0,0 +1,5 @@
+AxisState axis_requested_state
+---
+ActiveErrors active_errors
+AxisState axis_state
+ProcedureResult procedure_result