From a20d831ce3324946269bfed8b65e7ee56ea284d3 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Sun, 23 Mar 2025 16:11:57 +0800 Subject: [PATCH 1/4] AGV Sentry fixed --- .../meta_chassis_controller/CMakeLists.txt | 2 + .../agv_chassis_controller.hpp | 9 +-- .../agv_wheel_kinematics.hpp | 12 ++-- .../omni_chassis_controller.hpp | 17 +++-- .../src/agv_chassis_controller.cpp | 31 ++++----- .../src/agv_wheel_kinematics.cpp | 68 ++++++++----------- .../src/omni_chassis_controller.cpp | 2 +- .../urdf/sentry25/sentry.xacro | 4 +- meta_bringup/config/sentry25.yaml | 4 +- meta_bringup/launch/sentry25.launch.py | 10 +-- 10 files changed, 71 insertions(+), 88 deletions(-) diff --git a/decomposition/meta_chassis_controller/CMakeLists.txt b/decomposition/meta_chassis_controller/CMakeLists.txt index 5b9976b..2477bf9 100644 --- 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 index cef6f55..0122035 100644 --- 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 param_listener_; agv_chassis_controller::Params params_; - std::vector wheels_vel_; - std::vector wheels_pos_; - std::shared_ptr follow_pid_; std::vector> 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 index 7dab2c3..6f927ba 100644 --- 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,16 @@ class AgvWheelKinematics { AgvWheelKinematics(const double agv_wheel_center_x, const double agv_wheel_center_y, const double agv_wheel_radius); ~AgvWheelKinematics() = default; - // std::vector forward(const std::vector &wheels_vel); - void inverse(Eigen::VectorXd, std::vector & curr_pos, std::vector & curr_vel); - + std::pair, std::array> inverse(Eigen::VectorXd twist + // , const std::array & curr_pos, const std::array & curr_vel + ); + private: void init(); + std::array curr_target_pos_ = {0.0, 0.0, 0.0, 0.0}; + std::array curr_target_vel_ = {0.0, 0.0, 0.0, 0.0}; double agv_wheel_center_x_, agv_wheel_center_y_, agv_wheel_radius_, agv_radius_; - std::tuple get_output(double curr_pos, double curr_vel, double target_pos, double target_vel); - // Eigen::MatrixXd motion_mat_; + std::pair xy2polar(double curr_pos, double curr_vel, 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 index 250c357..d5cecc3 100644 --- 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 -#include #include -#include "behavior_interface/msg/chassis.hpp" +#include #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 #include "rclcpp/duration.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include #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 +#include +#include 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 index c9fdc2a..56239f7 100644 --- 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 #include #include #include @@ -8,7 +9,7 @@ #include #include "angles/angles.h" -#include "controller_interface/helpers.hpp" +#include #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(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( 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( 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,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_); + const auto [wheels_pos, wheels_vel] = agv_wheel_kinematics_->inverse(twist); - for(int i = 0; i < params_.agv_pos_joints.size(); i++){ // FIXME: Magical Number - double steer_pos_ref = wheels_pos_[static_cast(i)]; + for(size_t i = 0; i < 4; i++){ + std::cout << i << wheels_pos[i] << std::endl; + 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(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 index 2559976..35ed361 100644 --- a/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp +++ b/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp @@ -13,43 +13,45 @@ 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::tuple AgvWheelKinematics::get_output(double curr_pos, double curr_vel, double target_pos, double target_vel){ - if(target_vel == 0.0){ +std::pair AgvWheelKinematics::xy2polar(double curr_pos, double curr_vel, double target_x, double target_y) { + double target_vel = sqrt(target_x * target_x + target_y * target_y); + + // 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); + double target_angle = atan2(target_y, target_x); + double angle_diff = angles::shortest_angular_distance(curr_pos, target_angle); + 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}; + return {target_angle, target_vel}; + } else if(curr_vel < 0 && (angle_diff > M_PI / 2 || angle_diff < - M_PI / 2)){ + return {target_angle, target_vel}; + } else{ + // return {target_angle, target_vel}; + return {angles::normalize_angle(M_PI + target_angle), -target_vel}; } } -void AgvWheelKinematics::inverse(Eigen::VectorXd twist, std::vector & curr_pos, std::vector & curr_vel){ +std::pair, std::array> AgvWheelKinematics::inverse(Eigen::VectorXd twist + // , const std::array & curr_pos, const std::array & curr_vel +) { // 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 +61,18 @@ void AgvWheelKinematics::inverse(Eigen::VectorXd twist, std::vector & 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 [left_forward_pos, left_forward_vel] = xy2polar(curr_target_pos_[0], curr_target_vel_[0], left_foward_x, left_foward_y); + const auto [left_back_pos, left_back_vel] = xy2polar(curr_target_pos_[1], curr_target_vel_[1], left_back_x, left_back_y); + const auto [right_forward_pos, right_forward_vel] = xy2polar(curr_target_pos_[2], curr_target_vel_[2], right_forward_x, right_forward_y); + const auto [right_back_pos, right_back_vel] = xy2polar(curr_target_pos_[3], curr_target_vel_[3], right_back_x, right_back_y); - // 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); + std::array target_pos = {left_forward_pos, left_back_pos, right_forward_pos, right_back_pos}; + std::array target_vel = {left_forward_vel, left_back_vel, right_forward_vel, right_back_vel}; - 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); + curr_target_pos_ = target_pos; + curr_target_vel_ = target_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 \ No newline at end of file diff --git a/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp b/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp index c3c6166..1c1019a 100644 --- 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 #include "angles/angles.h" -#include "controller_interface/helpers.hpp" +#include #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 index 083c91c..a05a796 100644 --- a/decomposition/metav_description/urdf/sentry25/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry25/sentry.xacro @@ -39,7 +39,7 @@ meta_hardware/MetaRobotDjiMotorNetwork - can0 + can3 @@ -77,4 +77,4 @@ - \ No newline at end of file + diff --git a/meta_bringup/config/sentry25.yaml b/meta_bringup/config/sentry25.yaml index b86551d..5c47216 100644 --- 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 index 9d9405b..ea3d4e7 100644 --- a/meta_bringup/launch/sentry25.launch.py +++ b/meta_bringup/launch/sentry25.launch.py @@ -101,10 +101,10 @@ 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 @@ -141,7 +141,7 @@ def generate_launch_description(): # Load controllers *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), # dbus_container, - # dbus_control, + wfly_control, # dbus_vehicle, # ahrs_launch, ]) From 2b7f3ab491250cd813baa02cf83c94b1cae8f1e4 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Mon, 24 Mar 2025 00:16:07 +0800 Subject: [PATCH 2/4] Revise motor ID for sentry --- decomposition/metav_description/urdf/sentry25/sentry.xacro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/decomposition/metav_description/urdf/sentry25/sentry.xacro b/decomposition/metav_description/urdf/sentry25/sentry.xacro index a05a796..a2f918e 100644 --- a/decomposition/metav_description/urdf/sentry25/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry25/sentry.xacro @@ -42,9 +42,9 @@ can3 - - - + + + Date: Mon, 24 Mar 2025 00:57:18 +0800 Subject: [PATCH 3/4] Sentry fixed --- .../src/agv_chassis_controller.cpp | 1 - .../src/agv_wheel_kinematics.cpp | 15 +++------- .../urdf/sentry25/sentry.xacro | 8 +++--- meta_bringup/launch/sentry25.launch.py | 18 ++++++------ perception/wfly_control/src/wfly_sbus.cpp | 28 +++++++++---------- 5 files changed, 31 insertions(+), 39 deletions(-) diff --git a/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp b/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp index 56239f7..9ecd8f3 100644 --- a/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp +++ b/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp @@ -316,7 +316,6 @@ AgvChassisController::update_and_write_commands(const rclcpp::Time &time, const auto [wheels_pos, wheels_vel] = agv_wheel_kinematics_->inverse(twist); for(size_t i = 0; i < 4; i++){ - std::cout << i << wheels_pos[i] << std::endl; 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 + 4].get_value(); diff --git a/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp b/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp index 35ed361..2e6d172 100644 --- a/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp +++ b/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp @@ -15,12 +15,8 @@ AgvWheelKinematics::AgvWheelKinematics(const double agv_wheel_center_x, const do agv_radius_ = sqrt(agv_wheel_center_x * agv_wheel_center_x + agv_wheel_center_y * agv_wheel_center_y); } -// void AgvWheelKinematics::init() { - -// } - std::pair AgvWheelKinematics::xy2polar(double curr_pos, double curr_vel, double target_x, double target_y) { - double target_vel = sqrt(target_x * target_x + target_y * target_y); + double target_vel = sqrt(target_x * target_x + target_y * target_y) / agv_wheel_radius_; // If the target velocity is zero, atan2 is meaningless, we should preserve current position if (target_vel == 0.0) { @@ -29,13 +25,10 @@ std::pair AgvWheelKinematics::xy2polar(double curr_pos, double cu double target_angle = atan2(target_y, target_x); double angle_diff = angles::shortest_angular_distance(curr_pos, target_angle); - - if(curr_vel > 0 && angle_diff < M_PI / 2 && angle_diff > - M_PI / 2){ // FIXME: This is not correct here - return {target_angle, target_vel}; - } else if(curr_vel < 0 && (angle_diff > M_PI / 2 || angle_diff < - M_PI / 2)){ + + if (angle_diff < (M_PI / 2) && angle_diff > -(M_PI / 2) ) { // FIXME: This is not correct here return {target_angle, target_vel}; - } else{ - // return {target_angle, target_vel}; + } else { return {angles::normalize_angle(M_PI + target_angle), -target_vel}; } } diff --git a/decomposition/metav_description/urdf/sentry25/sentry.xacro b/decomposition/metav_description/urdf/sentry25/sentry.xacro index a2f918e..0852849 100644 --- a/decomposition/metav_description/urdf/sentry25/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry25/sentry.xacro @@ -41,10 +41,10 @@ meta_hardware/MetaRobotDjiMotorNetwork can3 - - - - + + + + ch1 - 0x400) / static_cast(0x29e); switch (sbus_frame->ch5) { case 0x69e: - wfly_msg_.sa = "down"; + wfly_msg_.sa = "DOWN"; break; case 0x161: - wfly_msg_.sa = "up"; + wfly_msg_.sa = "UP"; break; default: - wfly_msg_.sa = "up"; + wfly_msg_.sa = "UP"; break; } switch (sbus_frame->ch6) { case 0x69e: - wfly_msg_.sb = "down"; + wfly_msg_.sb = "DOWN"; break; case 0x400: - wfly_msg_.sb = "mid"; + wfly_msg_.sb = "MID"; break; case 0x161: - wfly_msg_.sb = "up"; + wfly_msg_.sb = "UP"; break; default: - wfly_msg_.sb = "up"; + wfly_msg_.sb = "UP"; break; } switch (sbus_frame->ch7) { case 0x69e: - wfly_msg_.sc = "down"; + wfly_msg_.sc = "DOWN"; break; case 0x400: - wfly_msg_.sc = "mid"; + wfly_msg_.sc = "MID"; break; case 0x161: - wfly_msg_.sc = "up"; + wfly_msg_.sc = "UP"; break; default: - wfly_msg_.sc = "up"; + wfly_msg_.sc = "UP"; break; } switch (sbus_frame->ch8) { case 0x69e: - wfly_msg_.sd = "down"; + wfly_msg_.sd = "DOWN"; break; case 0x161: - wfly_msg_.sd = "up"; + wfly_msg_.sd = "UP"; break; default: - wfly_msg_.sd = "up"; + wfly_msg_.sd = "UP"; break; } From a48cb8ee0b8c23b86850435921126064befc2270 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Mon, 24 Mar 2025 01:12:32 +0800 Subject: [PATCH 4/4] Make AGV chassis keep current position --- .../agv_wheel_kinematics.hpp | 8 ++------ .../src/agv_chassis_controller.cpp | 8 +++++++- .../src/agv_wheel_kinematics.cpp | 17 ++++++----------- 3 files changed, 15 insertions(+), 18 deletions(-) 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 index 6f927ba..7780f3e 100644 --- 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,16 +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::pair, std::array> inverse(Eigen::VectorXd twist - // , const std::array & curr_pos, const std::array & curr_vel - ); + std::pair, std::array> inverse(Eigen::VectorXd twist, const std::array & curr_pos); private: void init(); - std::array curr_target_pos_ = {0.0, 0.0, 0.0, 0.0}; - std::array curr_target_vel_ = {0.0, 0.0, 0.0, 0.0}; double agv_wheel_center_x_, agv_wheel_center_y_, agv_wheel_radius_, agv_radius_; - std::pair xy2polar(double curr_pos, double curr_vel, double target_x, double target_y); + std::pair xy2polar(double curr_pos, double target_x, double target_y); }; } // 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 index 9ecd8f3..49c69e9 100644 --- a/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp +++ b/decomposition/meta_chassis_controller/src/agv_chassis_controller.cpp @@ -313,7 +313,13 @@ AgvChassisController::update_and_write_commands(const rclcpp::Time &time, return controller_interface::return_type::ERROR; } - const auto [wheels_pos, wheels_vel] = agv_wheel_kinematics_->inverse(twist); + std::array 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(size_t i = 0; i < 4; i++){ double steer_pos_ref = wheels_pos[i]; diff --git a/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp b/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp index 2e6d172..50ff26f 100644 --- a/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp +++ b/decomposition/meta_chassis_controller/src/agv_wheel_kinematics.cpp @@ -15,7 +15,7 @@ AgvWheelKinematics::AgvWheelKinematics(const double agv_wheel_center_x, const do agv_radius_ = sqrt(agv_wheel_center_x * agv_wheel_center_x + agv_wheel_center_y * agv_wheel_center_y); } -std::pair AgvWheelKinematics::xy2polar(double curr_pos, double curr_vel, double target_x, double target_y) { +std::pair 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_; // If the target velocity is zero, atan2 is meaningless, we should preserve current position @@ -33,9 +33,7 @@ std::pair AgvWheelKinematics::xy2polar(double curr_pos, double cu } } -std::pair, std::array> AgvWheelKinematics::inverse(Eigen::VectorXd twist - // , const std::array & curr_pos, const std::array & curr_vel -) { +std::pair, std::array> AgvWheelKinematics::inverse(Eigen::VectorXd twist, const std::array & 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] @@ -54,17 +52,14 @@ std::pair, std::array> AgvWheelKinematics::inve double right_back_x = vx + rot_vel; double right_back_y = vy - rot_vel; - const auto [left_forward_pos, left_forward_vel] = xy2polar(curr_target_pos_[0], curr_target_vel_[0], left_foward_x, left_foward_y); - const auto [left_back_pos, left_back_vel] = xy2polar(curr_target_pos_[1], curr_target_vel_[1], left_back_x, left_back_y); - const auto [right_forward_pos, right_forward_vel] = xy2polar(curr_target_pos_[2], curr_target_vel_[2], right_forward_x, right_forward_y); - const auto [right_back_pos, right_back_vel] = xy2polar(curr_target_pos_[3], curr_target_vel_[3], right_back_x, right_back_y); + 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); std::array target_pos = {left_forward_pos, left_back_pos, right_forward_pos, right_back_pos}; std::array target_vel = {left_forward_vel, left_back_vel, right_forward_vel, right_back_vel}; - curr_target_pos_ = target_pos; - curr_target_vel_ = target_vel; - return {target_pos, target_vel}; }