adding set_command_mode

odriverobotics · Dec 17, 2024 · cdd54db · cdd54db
1 parent cb4b989
commit cdd54db
Showing 1 changed file with 30 additions and 1 deletion.
diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp
@@ -36,6 +36,7 @@ class ODriveHardwareInterface final : public hardware_interface::SystemInterface
 
 private:
     void on_can_msg(const can_frame& frame);
+    void set_command_mode();
 
     EpollEventLoop event_loop_;
     std::vector<Axis> axes_;
@@ -143,6 +144,8 @@ CallbackReturn ODriveHardwareInterface::on_activate(const State&) {
     // This can be called several seconds before the controller finishes starting.
     // Therefore we enable the ODrives only in perform_command_mode_switch().
 
+    set_command_mode();
+
     for (auto& axis : axes_) {
         Clear_Errors_msg_t msg1;
         msg1.Identify = 0;
@@ -229,7 +232,8 @@ return_type ODriveHardwareInterface::perform_command_mode_switch(
         std::array<std::pair<std::string, bool*>, 3> interfaces = {
             {{info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, &axis.pos_input_enabled_},
              {info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, &axis.vel_input_enabled_},
-             {info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, &axis.torque_input_enabled_}}};
+             {info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, &axis.torque_input_enabled_}}
+        };
 
         bool mode_switch = false;
 
@@ -344,6 +348,31 @@ void ODriveHardwareInterface::on_can_msg(const can_frame& frame) {
     }
 }
 
+void ODriveHardwareInterface::set_command_mode() {
+    for (auto& axis : axes_) {
+        Set_Controller_Mode_msg_t msg;
+        if (axis.pos_input_enabled_) {
+            RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to position control");
+            msg.Control_Mode = CONTROL_MODE_POSITION_CONTROL;
+            msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
+        } else if (axis.vel_input_enabled_) {
+            RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to velocity control");
+            msg.Control_Mode = CONTROL_MODE_VELOCITY_CONTROL;
+            msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
+        } else {
+            RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to torque control");
+            msg.Control_Mode = CONTROL_MODE_TORQUE_CONTROL;
+            msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
+        }
+
+        bool any_enabled = axis.pos_input_enabled_ || axis.vel_input_enabled_ || axis.torque_input_enabled_;
+
+        if (any_enabled) {
+            axis.send(msg); // Set control mode
+        }
+    }
+}
+
 void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) {
     uint8_t cmd = frame.can_id & 0x1f;