From 61cfeb8ec12faef328c5482966e411af24d708ea Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 31 Oct 2023 09:56:39 -0500 Subject: [PATCH] Expose driver enumerations in messages --- CMakeLists.txt | 9 ++++++++- README.md | 2 +- include/odrive_can_node.hpp | 8 ++++---- msg/ActiveErrors.msg | 26 ++++++++++++++++++++++++++ msg/AxisState.msg | 17 +++++++++++++++++ msg/ControlMessage.msg | 4 ++-- msg/ControlMode.msg | 17 +++++++++++++++++ msg/ControllerStatus.msg | 6 +++--- msg/InputMode.msg | 12 ++++++++++++ msg/ODriveStatus.msg | 4 ++-- msg/ProcedureResult.msg | 19 +++++++++++++++++++ src/odrive_can_node.cpp | 26 +++++++++++++------------- srv/AxisState.srv | 5 ----- srv/RequestAxisState.srv | 5 +++++ 14 files changed, 129 insertions(+), 31 deletions(-) create mode 100644 msg/ActiveErrors.msg create mode 100644 msg/AxisState.msg create mode 100644 msg/ControlMode.msg create mode 100644 msg/InputMode.msg create mode 100644 msg/ProcedureResult.msg delete mode 100644 srv/AxisState.srv create mode 100644 srv/RequestAxisState.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index a7be9bd..5aeb4a9 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/README.md b/README.md index 4d27083..680fcad 100644 --- a/README.md +++ b/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)) \ No newline at end of file + print(ProcedureResult(ctrl_stat.procedure_result)) diff --git a/include/odrive_can_node.hpp b/include/odrive_can_node.hpp index b151c8b..5ef5a0e 100644 --- a/include/odrive_can_node.hpp +++ b/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 @@ -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 request, std::shared_ptr response); + void service_callback(const std::shared_ptr request, std::shared_ptr 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::SharedPtr service_; + rclcpp::Service::SharedPtr service_; }; diff --git a/msg/ActiveErrors.msg b/msg/ActiveErrors.msg new file mode 100644 index 0000000..6bdde0d --- /dev/null +++ b/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 diff --git a/msg/AxisState.msg b/msg/AxisState.msg new file mode 100644 index 0000000..cf79415 --- /dev/null +++ b/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 diff --git a/msg/ControlMessage.msg b/msg/ControlMessage.msg index 1bfed9c..db332d8 100644 --- a/msg/ControlMessage.msg +++ b/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 diff --git a/msg/ControlMode.msg b/msg/ControlMode.msg new file mode 100644 index 0000000..9317af5 --- /dev/null +++ b/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 diff --git a/msg/ControllerStatus.msg b/msg/ControllerStatus.msg index 072365f..20fff7c 100644 --- a/msg/ControllerStatus.msg +++ b/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 diff --git a/msg/InputMode.msg b/msg/InputMode.msg new file mode 100644 index 0000000..52d39fd --- /dev/null +++ b/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 diff --git a/msg/ODriveStatus.msg b/msg/ODriveStatus.msg index 3800e59..2b9caf4 100644 --- a/msg/ODriveStatus.msg +++ b/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 diff --git a/msg/ProcedureResult.msg b/msg/ProcedureResult.msg new file mode 100644 index 0000000..b348d50 --- /dev/null +++ b/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 diff --git a/src/odrive_can_node.cpp b/src/odrive_can_node.cpp index 12bcf68..188730e 100644 --- a/src/odrive_can_node.cpp +++ b/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("control_message", ctrl_msg_qos, std::bind(&ODriveCanNode::subscriber_callback, this, _1)); rclcpp::QoS srv_qos(rclcpp::KeepAll{}); - service_ = rclcpp::Node::create_service("request_axis_state", std::bind(&ODriveCanNode::service_callback, this, _1, _2), srv_qos); + service_ = rclcpp::Node::create_service("request_axis_state", std::bind(&ODriveCanNode::service_callback, this, _1, _2), srv_qos); } void ODriveCanNode::deinit() { @@ -80,10 +80,10 @@ void ODriveCanNode::recv_callback(const can_frame& frame) { case CmdId::kHeartbeat: { if (!verify_length("kHeartbeat", 8, frame.len)) break; std::lock_guard guard(ctrl_stat_mutex_); - ctrl_stat_.active_errors = read_le(frame.data + 0); - ctrl_stat_.axis_state = read_le(frame.data + 4); - ctrl_stat_.procedure_result = read_le(frame.data + 5); - ctrl_stat_.trajectory_done_flag = read_le(frame.data + 6); + ctrl_stat_.active_errors.errors = read_le(frame.data + 0); + ctrl_stat_.axis_state.state = read_le(frame.data + 4); + ctrl_stat_.procedure_result.result = read_le(frame.data + 5); + ctrl_stat_.trajectory_done_flag = read_le(frame.data + 6); ctrl_pub_flag_ |= 0b0001; fresh_heartbeat_.notify_one(); break; @@ -91,8 +91,8 @@ void ODriveCanNode::recv_callback(const can_frame& frame) { case CmdId::kGetError: { if (!verify_length("kGetError", 8, frame.len)) break; std::lock_guard guard(odrv_stat_mutex_); - odrv_stat_.active_errors = read_le(frame.data + 0); - odrv_stat_.disarm_reason = read_le(frame.data + 4); + odrv_stat_.active_errors.errors = read_le(frame.data + 0); + odrv_stat_.disarm_reason.errors = read_le(frame.data + 4); odrv_pub_flag_ |= 0b001; break; } @@ -159,10 +159,10 @@ void ODriveCanNode::subscriber_callback(const ControlMessage::SharedPtr msg) { sub_evt_.set(); } -void ODriveCanNode::service_callback(const std::shared_ptr request, std::shared_ptr response) { +void ODriveCanNode::service_callback(const std::shared_ptr request, std::shared_ptr response) { { std::unique_lock 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(); @@ -170,7 +170,7 @@ void ODriveCanNode::service_callback(const std::shared_ptr r std::unique_lock 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 guard(ctrl_msg_mutex_); - write_le(ctrl_msg_.control_mode, frame.data); - write_le(ctrl_msg_.input_mode, frame.data + 4); - control_mode = ctrl_msg_.control_mode; + write_le(ctrl_msg_.control_mode.mode, frame.data); + write_le(ctrl_msg_.input_mode.mode, frame.data + 4); + control_mode = ctrl_msg_.control_mode.mode; } frame.len = 8; can_intf_.send_can_frame(frame); diff --git a/srv/AxisState.srv b/srv/AxisState.srv deleted file mode 100644 index 0eeefa8..0000000 --- a/srv/AxisState.srv +++ /dev/null @@ -1,5 +0,0 @@ -uint32 axis_requested_state ---- -uint32 active_errors -uint8 axis_state -uint8 procedure_result diff --git a/srv/RequestAxisState.srv b/srv/RequestAxisState.srv new file mode 100644 index 0000000..1cdb2c4 --- /dev/null +++ b/srv/RequestAxisState.srv @@ -0,0 +1,5 @@ +AxisState axis_requested_state +--- +ActiveErrors active_errors +AxisState axis_state +ProcedureResult procedure_result