Merge pull request #14 from nice-mee/master

nice-mee · web-flow · commit 2868b46ca3cb · 2025-05-06T23:41:11.000+08:00
Fix AGV chassis
diff --git a/decomposition/meta_chassis_controller/CMakeLists.txt b/decomposition/meta_chassis_controller/CMakeLists.txt
@@ -5,6 +5,8 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
   add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow)
 endif()
 
+set(CMAKE_CXX_STANDARD 20)
+
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
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
@@ -9,12 +9,12 @@
 #include "control_toolbox/pid_ros.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "meta_chassis_controller/agv_wheel_kinematics.hpp"
-#include "agv_chassis_controller_parameters.hpp"
+#include "meta_chassis_controller/agv_chassis_controller_parameters.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.h"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
 
 #include "control_msgs/msg/joint_controller_state.hpp"
 #include "geometry_msgs/msg/twist.hpp"
@@ -63,9 +63,6 @@ class AgvChassisController : public controller_interface::ChainableControllerInt
     std::shared_ptr<agv_chassis_controller::ParamListener> param_listener_;
     agv_chassis_controller::Params params_;
 
-    std::vector<double> wheels_vel_;
-    std::vector<double> wheels_pos_;
-
     std::shared_ptr<control_toolbox::PidROS> follow_pid_;
 
     std::vector<std::shared_ptr<control_toolbox::PidROS>> steer_pos2vel_pid_;
diff --git a/decomposition/meta_chassis_controller/include/meta_chassis_controller/agv_wheel_kinematics.hpp b/decomposition/meta_chassis_controller/include/meta_chassis_controller/agv_wheel_kinematics.hpp
@@ -10,14 +10,12 @@ class AgvWheelKinematics {
     AgvWheelKinematics(const double agv_wheel_center_x, const double agv_wheel_center_y, const double agv_wheel_radius); 
     ~AgvWheelKinematics() = default;
 
-    // std::vector<double> forward(const std::vector<double> &wheels_vel);
-    void inverse(Eigen::VectorXd, std::vector<double> & curr_pos, std::vector<double> & curr_vel);
-    
+    std::pair<std::array<double, 4>, std::array<double, 4>> inverse(Eigen::VectorXd twist, const std::array<double, 4> & curr_pos);
+
   private:
     void init();
     double agv_wheel_center_x_, agv_wheel_center_y_, agv_wheel_radius_, agv_radius_;
-    std::tuple<double, double> get_output(double curr_pos, double curr_vel, double target_pos, double target_vel);
-    // Eigen::MatrixXd motion_mat_;
+    std::pair<double, double> xy2polar(double curr_pos, double target_x, double target_y);
 };
 
 } // namespace meta_chassis_controller
diff --git a/decomposition/meta_chassis_controller/include/meta_chassis_controller/omni_chassis_controller.hpp b/decomposition/meta_chassis_controller/include/meta_chassis_controller/omni_chassis_controller.hpp
@@ -2,23 +2,22 @@
 #define OMNI_CHASSIS_CONTROLLER__OMNI_CHASSIS_CONTROLLER_HPP_
 
 #include <memory>
-#include <string>
 #include <vector>
 
-#include "behavior_interface/msg/chassis.hpp"
+#include <behavior_interface/msg/chassis.hpp>
 #include "control_toolbox/pid_ros.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "meta_chassis_controller/omni_wheel_kinematics.hpp"
-#include "omni_chassis_controller_parameters.hpp"
+#include <meta_chassis_controller/omni_chassis_controller_parameters.hpp>
 #include "rclcpp/duration.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.h"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
 
-#include "control_msgs/msg/joint_controller_state.hpp"
-#include "geometry_msgs/msg/twist.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
+#include <control_msgs/msg/joint_controller_state.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 namespace meta_chassis_controller {
 
diff --git a/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp b/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp
@@ -1,5 +1,6 @@
 #include "meta_chassis_controller/agv_chassis_controller.hpp"
 
+#include <Eigen/LU>
 #include <Eigen/src/Core/Matrix.h>
 #include <behavior_interface/msg/detail/chassis__struct.hpp>
 #include <limits>
@@ -8,7 +9,7 @@
 #include <vector>
 
 #include "angles/angles.h"
-#include "controller_interface/helpers.hpp"
+#include <controller_interface/helpers.hpp>
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/logging.hpp"
 
@@ -62,11 +63,6 @@ AgvChassisController::on_configure(const rclcpp_lifecycle::State & /*previous_st
     params_ = param_listener_->get_params();
 
     agv_wheel_kinematics_ = std::make_unique<AgvWheelKinematics>(params_.agv_wheel_center_x, params_.agv_wheel_center_y, params_.agv_wheel_radius);
-    // Set Wheels Velocities and Positions
-    wheels_vel_.resize(4);
-    wheels_pos_.resize(4);
-    std::fill(wheels_vel_.begin(), wheels_vel_.end(), 0);
-    std::fill(wheels_pos_.begin(), wheels_pos_.end(), 0);
 
     // Initialize PIDs
 
@@ -80,7 +76,7 @@ AgvChassisController::on_configure(const rclcpp_lifecycle::State & /*previous_st
     steer_pos2vel_pid_[3] = std::make_shared<control_toolbox::PidROS>(
         get_node(), "right_back_steer_pos2vel_gains", true);
 
-    for (int i = 0; i < steer_pos2vel_pid_.size(); i++) {
+    for (size_t i = 0; i < steer_pos2vel_pid_.size(); i++) {
         if (!steer_pos2vel_pid_[i]->initPid()) {
             RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for pos2vel");
             return controller_interface::CallbackReturn::FAILURE;
@@ -97,7 +93,7 @@ AgvChassisController::on_configure(const rclcpp_lifecycle::State & /*previous_st
     steer_vel2eff_pid_[3] = std::make_shared<control_toolbox::PidROS>(
         get_node(), "right_back_steer_vel2eff_gains", true);
 
-    for (int i = 0; i < steer_vel2eff_pid_.size(); i++) {
+    for (size_t i = 0; i < steer_vel2eff_pid_.size(); i++) {
         if (!steer_vel2eff_pid_[i]->initPid()) {
             RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for pos2vel");
             return controller_interface::CallbackReturn::FAILURE;
@@ -317,23 +313,27 @@ AgvChassisController::update_and_write_commands(const rclcpp::Time &time,
             return controller_interface::return_type::ERROR;
         }
 
-        agv_wheel_kinematics_->inverse(twist, wheels_pos_, wheels_vel_);
+        std::array<double, 4> curr_wheels_pos = {
+            state_interfaces_[1].get_value(),
+            state_interfaces_[2].get_value(),
+            state_interfaces_[3].get_value(),
+            state_interfaces_[4].get_value()
+        };
+        const auto [wheels_pos, wheels_vel] = agv_wheel_kinematics_->inverse(twist, curr_wheels_pos);
 
-        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)];
+        for(size_t i = 0; i < 4; i++){
+            double steer_pos_ref = wheels_pos[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_vel_fb = state_interfaces_[1 + i + 4].get_value();
             double steer_pos_err = angles::shortest_angular_distance(steer_pos_fb, steer_pos_ref);
             double steer_vel_ref = steer_pos2vel_pid_[i]->computeCommand(steer_pos_err, period);
-            steer_vel_ref = 8.0;
             double steer_vel_err = steer_vel_ref - steer_vel_fb;
             double steer_eff_cmd = steer_vel2eff_pid_[i]->computeCommand(steer_vel_err, period);
-            command_interfaces_[i + params_.agv_vel_joints.size()].set_value(steer_eff_cmd);
+            command_interfaces_[i + 4].set_value(steer_eff_cmd);
         }
-        
-        for (size_t i = 0; i < params_.agv_pos_joints.size(); i++) {
-            command_interfaces_[i].set_value(wheels_vel_[static_cast<Eigen::Index>(i)]);
-            
+
+        for (size_t i = 0; i < 4; i++) {
+            command_interfaces_[i].set_value(wheels_vel[i]);
         }
     }
 
diff --git a/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp b/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp
@@ -13,43 +13,36 @@ namespace meta_chassis_controller {
 AgvWheelKinematics::AgvWheelKinematics(const double agv_wheel_center_x, const double agv_wheel_center_y, const double agv_wheel_radius) :
     agv_wheel_center_x_(agv_wheel_center_x), agv_wheel_center_y_(agv_wheel_center_y), agv_wheel_radius_(agv_wheel_radius) {
     agv_radius_ = sqrt(agv_wheel_center_x * agv_wheel_center_x + agv_wheel_center_y * agv_wheel_center_y);
-    // init();
 }
 
-// void AgvWheelKinematics::init() {
-    
-// }
+std::pair<double,double> AgvWheelKinematics::xy2polar(double curr_pos, double target_x, double target_y) {
+    double target_vel = sqrt(target_x * target_x + target_y * target_y) / agv_wheel_radius_;
 
-std::tuple<double,double> AgvWheelKinematics::get_output(double curr_pos, double curr_vel, double target_pos, double target_vel){
-    if(target_vel == 0.0){
+    // If the target velocity is zero, atan2 is meaningless, we should preserve current position
+    if (target_vel == 0.0) {
         return {curr_pos, 0.0};
     }
 
-    double angle_diff = angles::shortest_angular_distance(curr_pos, target_pos);
-    if(curr_vel > 0 && angle_diff < M_PI / 2 && angle_diff > - M_PI / 2){       // FIXME: This is not correct here
-        return {target_pos, target_vel};
-    }else if(curr_vel < 0 && (angle_diff > M_PI / 2 || angle_diff < - M_PI / 2)){
-        return {target_pos, target_vel};
-    }else{
-        return {angles::normalize_angle(M_PI * 2 - target_pos), -target_vel};
+    double target_angle = atan2(target_y, target_x);
+    double angle_diff = angles::shortest_angular_distance(curr_pos, target_angle);
+
+    if (angle_diff < (M_PI / 2) && angle_diff > -(M_PI / 2) ) {       // FIXME: This is not correct here
+        return {target_angle, target_vel};
+    } else {
+        return {angles::normalize_angle(M_PI + target_angle), -target_vel};
     }
 }
 
-void AgvWheelKinematics::inverse(Eigen::VectorXd twist, std::vector<double> & curr_pos, std::vector<double> & curr_vel){
+std::pair<std::array<double, 4>, std::array<double, 4>> AgvWheelKinematics::inverse(Eigen::VectorXd twist, const std::array<double, 4> & curr_pos) {
     // twist: [vx, vy, wz]
     // curr_pos: [left_forward, left_back, right_forward, right_back]
     // curr_vel: [left_forward, left_back, right_forward, right_back]
 
-    if ( curr_pos.size() != 4 || curr_pos.size() != 4){
-        // RCLCPP_ERROR();
-        // std::raise(ERROR); // Raise an error
-    }
     double vx = twist[0];
     double vy = twist[1];
     double wz = twist[2];
-    double v = sqrt(vx * vx + vy * vy) / agv_wheel_radius_;
     double rot_vel = wz * agv_radius_;
-    
+
     double left_foward_x = vx - rot_vel;
     double left_foward_y = vy + rot_vel;
     double left_back_x = vx - rot_vel;
@@ -59,32 +52,15 @@ void AgvWheelKinematics::inverse(Eigen::VectorXd twist, std::vector<double> & cu
     double right_back_x = vx + rot_vel;
     double right_back_y = vy - rot_vel;
 
-    double left_forward_angle = atan2(left_foward_y, left_foward_x);
-    double left_back_angle = atan2(left_back_y, left_back_x);
-    double right_forward_angle = atan2(right_forward_y, right_forward_x);
-    double right_back_angle = atan2(right_back_y, right_back_x);
-
-    // const auto& [curr_pos[0], curr_vel[0]] = get_output(curr_pos[0], curr_vel[0], left_forward_angle, v);
-    // const auto& [curr_pos[1], curr_vel[1]] = get_output(curr_pos[1], curr_vel[1], left_back_angle, v);
-    // const auto& [curr_pos[2], curr_vel[2]] = get_output(curr_pos[2], curr_vel[2], right_forward_angle, v);
-    // const auto& [curr_pos[3], curr_vel[3]] = get_output(curr_pos[3], curr_vel[3], right_back_angle, v);
+    const auto [left_forward_pos, left_forward_vel] = xy2polar(curr_pos[0], left_foward_x, left_foward_y);
+    const auto [left_back_pos, left_back_vel] = xy2polar(curr_pos[1], left_back_x, left_back_y);
+    const auto [right_forward_pos, right_forward_vel] = xy2polar(curr_pos[2], right_forward_x, right_forward_y);
+    const auto [right_back_pos, right_back_vel] = xy2polar(curr_pos[3], right_back_x, right_back_y);
 
-    const auto& [left_forward_pos, left_forward_vel] = get_output(curr_pos[0], curr_vel[0], left_forward_angle, v);
-    const auto& [left_back_pos, left_back_vel] = get_output(curr_pos[1], curr_vel[1], left_back_angle, v);
-    const auto& [right_forward_pos, right_forward_vel] = get_output(curr_pos[2], curr_vel[2], right_forward_angle, v);
-    const auto& [right_back_pos, right_back_vel] = get_output(curr_pos[3], curr_vel[3], right_back_angle, v);
+    std::array<double, 4> target_pos = {left_forward_pos, left_back_pos, right_forward_pos, right_back_pos};
+    std::array<double, 4> target_vel = {left_forward_vel, left_back_vel, right_forward_vel, right_back_vel};
 
-    curr_pos[0] = left_forward_pos;
-    curr_pos[1] = left_back_pos;
-    curr_pos[2] = right_forward_pos;
-    curr_pos[3] = right_back_pos;
-    curr_vel[0] = left_forward_vel;
-    curr_vel[1] = left_back_vel;
-    curr_vel[2] = right_forward_vel;
-    curr_vel[3] = right_back_vel;
-    
-
-
-}   
+    return {target_pos, target_vel};
+}
 
 } // namespace meta_chassis_controller
diff --git a/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp b/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp
@@ -8,7 +8,7 @@
 #include <vector>
 
 #include "angles/angles.h"
-#include "controller_interface/helpers.hpp"
+#include <controller_interface/helpers.hpp>
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/logging.hpp"
 
diff --git a/decomposition/metav_description/urdf/sentry25/sentry.xacro b/decomposition/metav_description/urdf/sentry25/sentry.xacro
@@ -39,12 +39,12 @@
         <ros2_control name="dji_motors" type="system">
             <hardware>
                 <plugin>meta_hardware/MetaRobotDjiMotorNetwork</plugin>
-                <param name="can_network_name">can0</param>
+                <param name="can_network_name">can3</param>
             </hardware>
-            <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:wheel_transmission prefix="left_forward" mechanical_reduction="20.0" vel_motor_id="3" pos_motor_id="3" pos_offset="-1.9967"/>
+            <xacro:wheel_transmission prefix="left_back" mechanical_reduction="20.0" vel_motor_id="1" pos_motor_id = "1" pos_offset="0.6881"/>
+            <xacro:wheel_transmission prefix="right_forward" mechanical_reduction="20.0" vel_motor_id="4" pos_motor_id = "4" pos_offset="-2.2759"/>
+            <xacro:wheel_transmission prefix="right_back" mechanical_reduction="20.0" vel_motor_id="2" pos_motor_id = "2" pos_offset="-1.7896"/>
             <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"
@@ -77,4 +77,4 @@
     <xacro:if value="$(arg is_simulation)">
         <xacro:include filename="$(find metav_description)/urdf/sentry/sentry.gazebo.xacro" />
     </xacro:if>
-</robot>
+</robot>
diff --git a/meta_bringup/config/sentry25.yaml b/meta_bringup/config/sentry25.yaml
@@ -188,9 +188,9 @@ shoot_controller:
       bullet_loader_joint_vel2eff:
         { p: 1.0e-1, i: 5.0e-1, d: 0.0e-2, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true }
 
-dbus_control:
+wfly_control:
   ros__parameters:
-    dbus_port: "dbus_control"
+    sbus_port: "/dev/wfly_receiver"
 
 dbus_vehicle:
   ros__parameters:
diff --git a/meta_bringup/launch/sentry25.launch.py b/meta_bringup/launch/sentry25.launch.py
@@ -101,23 +101,23 @@ def generate_launch_description():
         # load_controller('shoot_controller')
     ]
 
-    dbus_control = Node(
-        package='dbus_control',
-        executable='dbus_control_node',
-        name='dbus_control',
+    wfly_control = Node(
+        package='wfly_control',
+        executable='wfly_control_node',
+        name='wfly_control',
         parameters=[robot_config],
         output='both',
         emulate_tty=True
     )
 
-    # dbus_vehicle = Node(
-    #     package='dbus_vehicle',
-    #     executable='dbus_vehicle_node',
-    #     name='dbus_vehicle',
-    #     output='both',
-    #     parameters=[robot_config],
-    #     emulate_tty=True
-    # )
+    dbus_vehicle = Node(
+        package='dbus_vehicle',
+        executable='dbus_vehicle_node',
+        name='dbus_vehicle',
+        output='both',
+        parameters=[robot_config],
+        emulate_tty=True
+    )
 
     # ahrs_launch = IncludeLaunchDescription(
     #     PythonLaunchDescriptionSource(
@@ -141,7 +141,7 @@ def generate_launch_description():
         # Load controllers
         *register_sequential_loading(load_joint_state_broadcaster, *load_controllers),
         # dbus_container,
-        # dbus_control,
-        # dbus_vehicle,
+        wfly_control,
+        dbus_vehicle,
         # ahrs_launch,
     ])
diff --git a/perception/wfly_control/src/wfly_sbus.cpp b/perception/wfly_control/src/wfly_sbus.cpp
