adding torque check

Author: RichFel

odrive_ros2_control/src/odrive_hardware_interface.cpp

@@ -327,10 +327,12 @@ bool ODriveHardwareInterface::set_axis_command_mode(Axis axis) {
         RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to velocity control");
         msg.Control_Mode = CONTROL_MODE_VELOCITY_CONTROL;
         msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
-    } else {
+    } else if (axis.torque_input_enabled_) {
         RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to torque control");
         msg.Control_Mode = CONTROL_MODE_TORQUE_CONTROL;
         msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
+    } else {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "No control mode specified");
     }
 
     bool any_enabled = axis.pos_input_enabled_ || axis.vel_input_enabled_ || axis.torque_input_enabled_;