agv chassis strange bug

Raventhatfly · Raventhatfly · commit 597b9f3798eb · 2025-03-12T20:58:39.000+08:00
diff --git a/decomposition/meta_chassis_controller/include/meta_chassis_controller/agv_chassis_controller.hpp b/decomposition/meta_chassis_controller/include/meta_chassis_controller/agv_chassis_controller.hpp
@@ -68,6 +68,9 @@ class AgvChassisController : public controller_interface::ChainableControllerInt
 
     std::shared_ptr<control_toolbox::PidROS> follow_pid_;
 
+    std::shared_ptr<control_toolbox::PidROS> steer_pos2vel_pid_;
+    std::shared_ptr<control_toolbox::PidROS> steer_vel2eff_pid_;
+
     rclcpp::Duration ref_timeout_ = rclcpp::Duration(0, 0);
     rclcpp::Subscription<ControllerReferenceMsgUnstamped>::SharedPtr twist_sub_;
     realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> ref_buf_;
diff --git a/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp b/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp
@@ -68,6 +68,23 @@ AgvChassisController::on_configure(const rclcpp_lifecycle::State & /*previous_st
     std::fill(wheels_vel_.begin(), wheels_vel_.end(), 0);
     std::fill(wheels_pos_.begin(), wheels_pos_.end(), 0);
 
+    // Initialize PIDs
+    steer_pos2vel_pid_ = std::make_shared<control_toolbox::PidROS>(
+        get_node(), "steer_pos2vel_gains", true);
+
+    if (!steer_pos2vel_pid_->initPid()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for pos2vel");
+        return controller_interface::CallbackReturn::FAILURE;
+    }
+
+    steer_vel2eff_pid_ = std::make_shared<control_toolbox::PidROS>(
+        get_node(), "steer_vel2eff_gains", true);
+
+    if (!steer_vel2eff_pid_->initPid()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for vel2eff");
+        return controller_interface::CallbackReturn::FAILURE;
+    }
+
     // topics QoS
     auto subscribers_qos = rclcpp::SystemDefaultsQoS();
     subscribers_qos.keep_last(1);
@@ -142,6 +159,7 @@ AgvChassisController::command_interface_configuration() const {
     }
     for (const auto &joint : params_.agv_pos_joints) {
         command_interfaces_config.names.push_back(joint + "/" + HW_IF_EFFORT);
+        std::cout << "command interfaces: " << params_.agv_vel_joints.size() + params_.agv_pos_joints.size() << std::endl;
     }
 
     return command_interfaces_config;
@@ -154,10 +172,22 @@ AgvChassisController::state_interface_configuration() const {
         controller_interface::interface_configuration_type::INDIVIDUAL;
 
     // Joint position state of yaw gimbal is required
-    state_interfaces_config.names.reserve(1);
+    state_interfaces_config.names.reserve(9);
     state_interfaces_config.names.push_back(params_.yaw_gimbal_joint + "/" +
                                             HW_IF_POSITION);
 
+    for(const auto &joint : params_.agv_pos_joints){
+        state_interfaces_config.names.push_back(joint + "/" +
+                                            HW_IF_POSITION);
+    }
+
+    for(const auto &joint : params_.agv_pos_joints){
+        state_interfaces_config.names.push_back(joint + "/" +
+                                            HW_IF_VELOCITY);
+        std::cout <<"state interfaces: " << joint << std::endl;
+    }
+
+
     return state_interfaces_config;
 }
 
@@ -268,11 +298,22 @@ AgvChassisController::update_and_write_commands(const rclcpp::Time &time,
             return controller_interface::return_type::ERROR;
         }
 
-        agv_wheel_kinematics_->inverse(twist, wheels_pos_, wheels_vel_);
+        // agv_wheel_kinematics_->inverse(twist, wheels_pos_, wheels_vel_);
+
+        for(int i = 0; i < params_.agv_pos_joints.size(); i++){         // FIXME: Magical Number
+            double steer_pos_ref = wheels_pos_[static_cast<Eigen::Index>(i)];
+            double steer_pos_fb = state_interfaces_[1 + i].get_value();
+            double steer_vel_fb = state_interfaces_[1 + i + params_.agv_pos_joints.size()].get_value();
+            double steer_pos_err = angles::shortest_angular_distance(steer_pos_fb, steer_pos_ref);
+            double steer_vel_ref = steer_pos2vel_pid_->computeCommand(steer_pos_err, period);
+            double steer_vel_err = steer_vel_ref - steer_vel_fb;
+            double steer_eff_cmd = steer_vel2eff_pid_->computeCommand(steer_vel_err, period);
+            command_interfaces_[i + params_.agv_vel_joints.size()].set_value(steer_eff_cmd);
+        }
         
-        for (size_t i = 0; i < 4; i++) {
+        for (size_t i = 0; i < params_.agv_pos_joints.size(); i++) {
             command_interfaces_[i].set_value(wheels_vel_[static_cast<Eigen::Index>(i)]);
-            command_interfaces_[i + 4].set_value(wheels_pos_[static_cast<Eigen::Index>(i)]);
+            
         }
     }
 
diff --git a/decomposition/meta_chassis_controller/src/agv_chassis_controller.yaml b/decomposition/meta_chassis_controller/src/agv_chassis_controller.yaml
@@ -50,5 +50,17 @@ agv_chassis_controller:
       default_value: 0.0,
       description: "Specifies target of the follow PID controller. Setting 0.0 meaning the car is always moving towards the front. Usually 0.7853981634(45 degree) gives the maximum speed.",
     }
+  steer_pos2vel_gains:
+    {
+      type: double,
+      default_value: 0.0,
+      description: "Specifies target of the follow PID controller. Setting 0.0 meaning the car is always moving towards the front. Usually 0.7853981634(45 degree) gives the maximum speed.",
+    }
+  steer_vel2eff_gains:
+    {
+      type: double,
+      default_value: 0.0,
+      description: "Specifies target of the follow PID controller. Setting 0.0 meaning the car is always moving towards the front. Usually 0.7853981634(45 degree) gives the maximum speed.",
+    }
   reference_timeout:
     { type: double, default_value: 0.1, description: "Specifies timeout for the reference." }
diff --git a/decomposition/metav_description/urdf/common/steer_wheel.xacro b/decomposition/metav_description/urdf/common/steer_wheel.xacro
@@ -63,15 +63,15 @@
 
     </xacro:macro>
 
-    <xacro:macro name="wheel_transmission" params="prefix mechanical_reduction motor_id">
+    <xacro:macro name="wheel_transmission" params="prefix mechanical_reduction vel_motor_id pos_motor_id pos_offset">
         <joint name="${prefix}_vel_joint">
             <command_interface name="effort">
             </command_interface>
             <state_interface name="position" />
             <state_interface name="velocity" />
             <state_interface name="effort" />
             <param name="motor_model">M3508</param>
-            <param name="motor_id">${motor_id}</param>
+            <param name="motor_id">${vel_motor_id}</param>
             <param name="mechanical_reduction">${mechanical_reduction}</param>
             <param name="offset">0.0</param>
         </joint>
@@ -82,9 +82,9 @@
             <state_interface name="velocity" />
             <state_interface name="effort" />
             <param name="motor_model">GM6020</param>
-            <param name="motor_id">${motor_id}</param>
-            <param name="mechanical_reduction">${mechanical_reduction}</param>
-            <param name="offset">0.0</param>
+            <param name="motor_id">${pos_motor_id}</param>
+            <param name="mechanical_reduction">1</param>
+            <param name="offset">${pos_offset}</param>
         </joint>
     </xacro:macro>
 </robot>
diff --git a/decomposition/metav_description/urdf/sentry25/sentry.xacro b/decomposition/metav_description/urdf/sentry25/sentry.xacro
@@ -39,22 +39,22 @@
         <ros2_control name="dji_motors" type="system">
             <hardware>
                 <plugin>meta_hardware/MetaRobotDjiMotorNetwork</plugin>
-                <param name="can_network_name">can1</param>
+                <param name="can_network_name">can0</param>
             </hardware>
-            <xacro:wheel_transmission prefix="left_forward" mechanical_reduction="20.0" motor_id="1" />
-            <xacro:wheel_transmission prefix="left_back" mechanical_reduction="20.0" motor_id="3" />
-            <xacro:wheel_transmission prefix="right_forward" mechanical_reduction="20.0" motor_id="4" />
-            <xacro:wheel_transmission prefix="right_back" mechanical_reduction="20.0" motor_id="2" />
+            <xacro:wheel_transmission prefix="left_forward" mechanical_reduction="20.0" vel_motor_id="3" pos_motor_id="3" pos_offset="-0.0609"/>
+            <xacro:wheel_transmission prefix="left_back" mechanical_reduction="20.0" vel_motor_id="2" pos_motor_id = "1" pos_offset="0.1227"/>
+            <xacro:wheel_transmission prefix="right_forward" mechanical_reduction="20.0" vel_motor_id="4" pos_motor_id = "2" pos_offset="-0.0356"/>
+            <xacro:wheel_transmission prefix="right_back" mechanical_reduction="20.0" vel_motor_id="1" pos_motor_id = "4" pos_offset="-0.0694"/>
             <xacro:gimbal_transmission prefix="yaw" mechanical_reduction="1.0" offset="0.25"
                 motor_id="5" />
             <xacro:gimbal_transmission prefix="pitch" mechanical_reduction="-1.0" offset="0.0"
                 motor_id="6" />
-            <xacro:shooter_transmission prefix="fric1" mechanical_reduction="1.0" offset="0.25"
+            <!-- <xacro:shooter_transmission prefix="fric1" mechanical_reduction="1.0" offset="0.25"
                 motor_id="5" />
             <xacro:shooter_transmission prefix="fric2" mechanical_reduction="-1.0" offset="0.0"
                 motor_id="6" />
             <xacro:shooter_transmission prefix="loader" mechanical_reduction="36.0" offset="0.0"
-                motor_id="7" />
+                motor_id="7" /> -->
         </ros2_control>
     </xacro:unless>
 
diff --git a/meta_bringup/config/sentry25.yaml b/meta_bringup/config/sentry25.yaml
@@ -54,10 +54,20 @@ agv_chassis_controller:
 
     follow_pid_target: 0.7853981634
 
-    steer_pos_pos2vel_gains:
-      { p: 4.0, i: 2.0e-1, d: 0.0, i_clamp_max: 3.0, i_clamp_min: -3.0, antiwindup: true }
-    steer_pos_vel2eff_gains:
-      { p: 4.0, i: 2.0e-1, d: 0.0, i_clamp_max: 3.0, i_clamp_min: -3.0, antiwindup: true }
+    # steer_pos2vel_gains:
+    #   { p: 45.0, i: 5.5, d: 1.0E4, i_clamp_max: 3.0, i_clamp_min: -3.0, antiwindup: true }
+    # steer_vel2eff_gains:
+    #   { p: 1.0, i: 35.0, d: 0.0, i_clamp_max: 3.0, i_clamp_min: -3.0, antiwindup: true }
+
+    steer_pos2vel_gains:
+      # { p: 7.0, i: 0.0, d: 0.0, i_clamp_max: 3.0, i_clamp_min: -3.0, antiwindup: true }
+      { p: 0.0, i: 0.0, d: 0.0 }
+    steer_vel2eff_gains:
+      { p: 1.0e-2, i: 2.5e-1, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true }
+    # steer_pos2vel_gains:
+    #   { p: 0.0, i: 0.0, d: 0.0, i_clamp_max: 3.0, i_clamp_min: -3.0, antiwindup: true }
+    # steer_vel2eff_gains:
+    #   { p: 1.0, i: 0.0, d: 0.0, i_clamp_max: 3.0, i_clamp_min: -3.0, antiwindup: true }
 
 
     reference_timeout: 0.2
diff --git a/meta_bringup/launch/sentry25.launch.py b/meta_bringup/launch/sentry25.launch.py
@@ -93,7 +93,7 @@ def generate_launch_description():
     # Order in this list is IMPORTANT
     load_controllers = [
         load_controller('wheels_pid_controller'),
-        load_controller('steer_pos2vel_pid_controller'),
+        # load_controller('steer_pos2vel_pid_controller'),
         # load_controller('steer_vel2eff_pid_controller'),
         
         # load_controller('gimbal_controller'),
