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..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,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 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); + private: void init(); 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 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..49c69e9 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,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 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(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(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..50ff26f 100644 --- 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 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 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 & curr_pos, std::vector & 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] - 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 & 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 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_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..0852849 100644 --- a/decomposition/metav_description/urdf/sentry25/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry25/sentry.xacro @@ -39,12 +39,12 @@ meta_hardware/MetaRobotDjiMotorNetwork - can0 + can3 - - - - + + + + - \ 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..819f6ee 100644 --- 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 index ec86d93..da86091 100644 --- a/perception/wfly_control/src/wfly_sbus.cpp +++ b/perception/wfly_control/src/wfly_sbus.cpp @@ -115,52 +115,52 @@ void WflySbus::process_packet(const uint8_t* data) { wfly_msg_.rs_y = (sbus_frame->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; }