diff --git a/.clang-format b/.clang-format index 994474914..5c1eec782 100644 --- a/.clang-format +++ b/.clang-format @@ -13,27 +13,25 @@ MaxEmptyLinesToKeep: 1 AlignAfterOpenBracket: AlwaysBreak -AlignArrayOfStructures: Right - AlignConsecutiveAssignments: - Enabled: false + Enabled: false AlignConsecutiveBitFields: - Enabled: true - AcrossEmptyLines: false - AcrossComments: false + Enabled: true + AcrossEmptyLines: false + AcrossComments: false AlignConsecutiveDeclarations: - Enabled: false + Enabled: false AlignConsecutiveMacros: - Enabled: true - AcrossEmptyLines: false - AcrossComments: false + Enabled: true + AcrossEmptyLines: false + AcrossComments: false # AlignConsecutiveShortCaseStatements: # Enabled: false AlignEscapedNewlines: Left AlignOperands: AlignAfterOperator AlignTrailingComments: - Kind: Always - OverEmptyLines: 64 + Kind: Always + OverEmptyLines: 64 PointerAlignment: Left AllowAllArgumentsOnNextLine: true diff --git a/.script/remote-status b/.script/remote-status new file mode 100755 index 000000000..f5805e05c --- /dev/null +++ b/.script/remote-status @@ -0,0 +1,65 @@ +#!/bin/env bash + +set -euo pipefail + +# 定义需要查询的服务列表 +services=( + "/rmcs/service/robot_status" +) + +# 使用 bash 远程执行，并传递服务列表作为参数 +ssh remote bash -s -- "${services[@]}" <<'EOF' +set -euo pipefail + +# 1. 环境准备 +set +u +if [ -f ~/env_setup.bash ]; then + source ~/env_setup.bash +fi +set -u + +# 获取所有可用服务列表 +all_services="$(ros2 service list 2>/dev/null || true)" +input_services=("${@}") + +# 2. 定义单个服务查询函数 +call_status_service() { + local service="$1" + + if ! printf "%s\n" "$all_services" | grep -Fxq "$service"; then + printf "[warn] service not found: %s\n\n" "$service" + return + fi + + printf "=== %s ===\n" "$service" + local raw + raw="$(ros2 service call "$service" std_srvs/srv/Trigger "{}" 2>&1 || true)" + + local msg + msg="$(printf "%s\n" "$raw" | sed -n "s/.*message='\(.*\)'.*/\1/p")" + + if [[ -z "$msg" ]]; then + printf "[warn] failed to parse message: %s\n%s\n" "$service" "$raw" + else + printf "%b\n" "$msg" + fi + printf "\n" +} + +# 3. 并发执行逻辑 +tmp_dir=$(mktemp -d) +trap 'rm -rf "${tmp_dir}"' EXIT + +for i in "${!input_services[@]}"; do + call_status_service "${input_services[$i]}" > "${tmp_dir}/${i}" 2>&1 & +done + +wait + +# 4. 按原始顺序汇总输出 +for i in "${!input_services[@]}"; do + if [ -f "${tmp_dir}/${i}" ]; then + cat "${tmp_dir}/${i}" + fi +done +EOF diff --git a/Dockerfile b/Dockerfile index 5fce6b00f..f35ba3ae3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -30,8 +30,9 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libceres-dev \ ros-$ROS_DISTRO-rviz2 \ ros-$ROS_DISTRO-foxglove-bridge \ - dotnet-sdk-8.0 \ ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs && \ + ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-msgs \ + lua5.4 liblua5.4-0 liblua5.4-dev && \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml new file mode 100644 index 000000000..85e636c1c --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -0,0 +1,164 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Sentry -> sentry_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + + - rmcs_core::controller::gimbal::EccentricDualYaw -> gimbal_controller + + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs_core::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller + + # - rmcs::navigation::Navigation -> rmcs_navigation + +# The positive direction is the one that battery exists +sentry_hardware: + ros__parameters: + board_serial_top_board: "af-da30" + board_serial_bottom_board: "d4-2184" + board_serial_gimbal_board: "d4-1d2b" + bottom_yaw_motor_zero_point: 26163 + top_yaw_motor_zero_point: 4540 + pitch_motor_zero_point: 1446 + left_front_zero_point: 7131 + left_back_zero_point: 3740 + right_back_zero_point: 1327 + right_front_zero_point: 5147 + +rmcs_navigation: + ros__parameters: + # 策略名称： + # - fast-push-output "速推前哨站" + # - kill-robots "杀伤优先" + decision: "fast-push-output" + command_vel_name: "/cmd_vel" + mock_context: false + endpoint: "test" + enable_goal_topic_forward: true + +gimbal_controller: + ros__parameters: + upper_limit: -0.39518 + lower_limit: 0.36 + + top_yaw_angle_kp: 30.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 2.160 + top_yaw_velocity_ki: 0.0 + top_yaw_velocity_kd: 0.0 + + bottom_yaw_angle_kp: 8.8 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 22.49 + bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kd: 0.0 + + pitch_angle_kp: 40.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.01 + pitch_velocity_kp: 2.5 + pitch_velocity_ki: 0.0 + pitch_velocity_kd: 0.0 + + k_top_to_bottom: -1.0 + + bottom_yaw_viscous_ff_gain: 0.002495 + # bottom_yaw_coulomb_ff_gain: 0.457343 + # bottom_yaw_coulomb_ff_tanh_gain: 100.0 + top_yaw_viscous_ff_gain: 0.231 + # top_yaw_coulomb_ff_gain: 1.12 + # top_yaw_coulomb_ff_tanh_gain: 100.0 + pitch_viscous_ff_gain: 0.33 + # pitch_coulomb_ff_gain: 0.95 + # pitch_coulomb_ff_tanh_gain: 100.0 + pitch_gravity_ff_gain: 2.128 + pitch_gravity_ff_phase: 1.438 + +chassis_controller: + ros__parameters: + navigation_velocity_scale: 1.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 630.0 + - 630.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 9.0 + shot_frequency: 28.0 + safe_shot_frequency: 10.0 + eject_frequency: 15.0 + eject_time: 0.15 + deep_eject_frequency: 15.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.283 + ki: 0.0 + kd: 0.0 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 0.77852676 + vehicle_radius: 0.26870058 + wheel_radius: 0.055 + friction_coefficient: 0.666 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + chassis_translation_kp: 8.0 + chassis_translation_ki: 0.0 + chassis_translation_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dd3c85799..36e7e5706 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.0.0/librmcs-sdk-src-3.0.0.zip - URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0/librmcs-sdk-src-3.1.0.zip + URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 410745520..4312d3341 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -11,6 +11,7 @@ rclcpp std_msgs + std_srvs pluginlib tf2 tf2_ros diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f7847151c..db9ff7c45 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -2,6 +2,7 @@ + @@ -10,6 +11,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 80d7a72e3..739b58592 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -145,4 +145,4 @@ class DualYawController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp new file mode 100644 index 000000000..4e6c4e72d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -0,0 +1,308 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::gimbal { +using namespace rmcs_description; + +class EccentricDualYaw + : public rmcs_executor::Component + , public rclcpp::Node { +public: + EccentricDualYaw() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} {} + + auto before_updating() -> void override { + enter_disabled_state(); + previous_actual_yaw_ = current_barrel_yaw_pitch().first; + previous_yaw_timestamp_ = *input_.timestamp; + } + + auto update() -> void override { + const auto actual_yaw_pitch = current_barrel_yaw_pitch(); + *output_.yaw_angle = *input_.bottom_yaw_angle; + *output_.yaw_velocity = compute_actual_yaw_velocity(actual_yaw_pitch.first); + + if (!input_.enable_control()) { + enter_disabled_state(); + return; + } + + const double yaw_shift = kJoystickSensitivity * input_.joystick_left->y() + + kMouseSensitivity * input_.mouse_velocity->y(); + const double pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() + - kMouseSensitivity * input_.mouse_velocity->x(); + + manual_bottom_yaw_target_ = limit_rad(manual_bottom_yaw_target_ + yaw_shift); + manual_pitch_target_ = + std::clamp(manual_pitch_target_ + pitch_shift, upper_limit_, lower_limit_); + + const auto manual_target = ControlTarget{ + .bottom_yaw = {.target = manual_bottom_yaw_target_}, + .top_yaw = {.target = 0.0}, + .pitch = {.target = manual_pitch_target_}, + }; + apply_control(manual_target); + } + +private: + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kJoystickSensitivity = 0.006; + static constexpr double kMouseSensitivity = 0.5; + + const double upper_limit_{get_parameter("upper_limit").as_double()}; + const double lower_limit_{get_parameter("lower_limit").as_double()}; + + const double bottom_yaw_viscous_ff_gain_{get_parameter_or("bottom_yaw_viscous_ff_gain", 0.0)}; + const double bottom_yaw_coulomb_ff_gain_{get_parameter_or("bottom_yaw_coulomb_ff_gain", 0.0)}; + const double bottom_yaw_coulomb_ff_tanh_gain_{ + get_parameter_or("bottom_yaw_coulomb_ff_tanh_gain", 100.0)}; + const double k_top_to_bottom_{get_parameter_or("k_top_to_bottom", 0.0)}; + const double top_yaw_viscous_ff_gain_{get_parameter_or("top_yaw_viscous_ff_gain", 0.0)}; + const double top_yaw_coulomb_ff_gain_{get_parameter_or("top_yaw_coulomb_ff_gain", 0.0)}; + const double top_yaw_coulomb_ff_tanh_gain_{ + get_parameter_or("top_yaw_coulomb_ff_tanh_gain", 100.0)}; + const double pitch_viscous_ff_gain_{get_parameter_or("pitch_viscous_ff_gain", 0.0)}; + const double pitch_coulomb_ff_gain_{get_parameter_or("pitch_coulomb_ff_gain", 0.0)}; + const double pitch_coulomb_ff_tanh_gain_{get_parameter_or("pitch_coulomb_ff_tanh_gain", 100.0)}; + const double pitch_gravity_ff_gain_{get_parameter_or("pitch_gravity_ff_gain", 0.0)}; + const double pitch_gravity_ff_phase_{get_parameter_or("pitch_gravity_ff_phase", 0.0)}; + + struct AxisCommand { + double target = 0.0; + double velocity_ff = 0.0; + double acceleration_ff = 0.0; + }; + struct ControlTarget { + AxisCommand bottom_yaw; + AxisCommand top_yaw; + AxisCommand pitch; + }; + + struct Input { + explicit Input(rmcs_executor::Component& component) { + component.register_input("/remote/joystick/left", joystick_left); + component.register_input("/remote/switch/right", switch_right); + component.register_input("/remote/switch/left", switch_left); + component.register_input("/remote/mouse/velocity", mouse_velocity); + + component.register_input("/predefined/timestamp", timestamp); + component.register_input("/tf", tf); + + component.register_input("/gimbal/top_yaw/angle", top_yaw_angle); + component.register_input("/gimbal/top_yaw/velocity", top_yaw_velocity); + component.register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle); + component.register_input("/gimbal/bottom_yaw/velocity", bottom_yaw_velocity); + component.register_input("/gimbal/pitch/angle", pitch_angle); + component.register_input("/gimbal/pitch/velocity", pitch_velocity); + component.register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu); + } + + auto enable_control() const noexcept -> bool { + using namespace rmcs_msgs; + if ((*switch_left == Switch::UNKNOWN || *switch_right == Switch::UNKNOWN) + || (*switch_left == Switch::DOWN && *switch_right == Switch::DOWN)) { + return false; + } + return true; + } + + InputInterface joystick_left; + InputInterface switch_right; + InputInterface switch_left; + InputInterface mouse_velocity; + + InputInterface timestamp; + InputInterface tf; + + InputInterface top_yaw_angle; + InputInterface top_yaw_velocity; + InputInterface bottom_yaw_angle; + InputInterface bottom_yaw_velocity; + InputInterface pitch_angle; + InputInterface pitch_velocity; + InputInterface chassis_yaw_velocity_imu; + } input_{*this}; + + struct Output { + explicit Output(rmcs_executor::Component& component) { + component.register_output( + "/gimbal/top_yaw/control_torque", top_yaw_control_torque, kNaN); + component.register_output( + "/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque, kNaN); + component.register_output("/gimbal/pitch/control_torque", pitch_control_torque, kNaN); + + component.register_output( + "/gimbal/yaw/control_angle_error", yaw_control_angle_error, kNaN); + component.register_output("/gimbal/yaw/angle", yaw_angle, 0.0); + component.register_output("/gimbal/yaw/velocity", yaw_velocity, 0.0); + } + + OutputInterface top_yaw_control_torque; + OutputInterface bottom_yaw_control_torque; + OutputInterface pitch_control_torque; + + OutputInterface yaw_control_angle_error; + OutputInterface yaw_angle; + OutputInterface yaw_velocity; + } output_{*this}; + + pid::PidCalculator top_yaw_angle_pid_{pid::make_pid_calculator(*this, "top_yaw_angle_")}; + pid::PidCalculator top_yaw_velocity_pid_{pid::make_pid_calculator(*this, "top_yaw_velocity_")}; + pid::PidCalculator bottom_yaw_angle_pid_{pid::make_pid_calculator(*this, "bottom_yaw_angle_")}; + pid::PidCalculator bottom_yaw_velocity_pid_{ + pid::make_pid_calculator(*this, "bottom_yaw_velocity_")}; + pid::PidCalculator pitch_angle_pid_{pid::make_pid_calculator(*this, "pitch_angle_")}; + pid::PidCalculator pitch_velocity_pid_{pid::make_pid_calculator(*this, "pitch_velocity_")}; + + double manual_bottom_yaw_target_ = 0.0; + double manual_pitch_target_ = 0.0; + double previous_actual_yaw_ = 0.0; + std::chrono::steady_clock::time_point previous_yaw_timestamp_{}; + + static constexpr auto limit_rad(double angle) -> double { + constexpr double kPi = std::numbers::pi_v; + while (angle > kPi) + angle -= 2.0 * kPi; + while (angle <= -kPi) + angle += 2.0 * kPi; + return angle; + } + + auto reset_all_controls() -> void { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + + *output_.top_yaw_control_torque = kNaN; + *output_.bottom_yaw_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; + } + + auto enter_disabled_state() -> void { + reset_all_controls(); + + manual_bottom_yaw_target_ = current_bottom_world_yaw(); + manual_pitch_target_ = + std::clamp(limit_rad(*input_.pitch_angle), upper_limit_, lower_limit_); + + *output_.yaw_control_angle_error = kNaN; + } + + auto compute_actual_yaw_velocity(double actual_yaw) -> double { + const auto now = *input_.timestamp; + const double dt = std::chrono::duration(now - previous_yaw_timestamp_).count(); + double velocity = 0.0; + if (dt > 1e-6) + velocity = limit_rad(actual_yaw - previous_actual_yaw_) / dt; + previous_actual_yaw_ = actual_yaw; + previous_yaw_timestamp_ = now; + return velocity; + } + + auto current_barrel_yaw_pitch() const -> std::pair { + auto direction = fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *input_.tf); + Eigen::Vector3d vector = *direction; + if (vector.norm() > 1e-9) + vector.normalize(); + else + vector = Eigen::Vector3d::UnitX(); + const double xy_norm = std::hypot(vector.x(), vector.y()); + return {std::atan2(vector.y(), vector.x()), std::atan2(-vector.z(), xy_norm)}; + } + + auto current_bottom_world_yaw() const -> double { + auto direction = fast_tf::cast( + BottomYawLink::DirectionVector{Eigen::Vector3d::UnitX()}, *input_.tf); + Eigen::Vector3d vector = *direction; + vector.z() = 0.0; + if (vector.norm() > 1e-9) + vector.normalize(); + else + vector = Eigen::Vector3d::UnitX(); + return std::atan2(vector.y(), vector.x()); + } + + auto apply_control(const ControlTarget& target) -> void { + + constexpr auto friction_feedforward = [](double viscous_gain, double coulomb_gain, + double tanh_gain, double velocity) -> double { + return (viscous_gain * velocity) + (coulomb_gain * std::tanh(tanh_gain * velocity)); + }; + + const double current_bottom_angle = current_bottom_world_yaw(); + const double current_bottom_velocity = + *input_.bottom_yaw_velocity + *input_.chassis_yaw_velocity_imu; + const double current_top_angle = limit_rad(*input_.top_yaw_angle); + const double current_pitch_angle = limit_rad(*input_.pitch_angle); + + const double bottom_yaw_error = limit_rad(target.bottom_yaw.target - current_bottom_angle); + const double top_yaw_error = limit_rad(target.top_yaw.target - current_top_angle); + const double pitch_error = limit_rad(target.pitch.target - current_pitch_angle); + + const double bottom_velocity_ref = + bottom_yaw_angle_pid_.update(bottom_yaw_error) + target.bottom_yaw.velocity_ff; + const double top_velocity_ref = + top_yaw_angle_pid_.update(top_yaw_error) + target.top_yaw.velocity_ff; + const double pitch_velocity_ref = + pitch_angle_pid_.update(pitch_error) + target.pitch.velocity_ff; + + const double bottom_world_velocity_ff = + target.bottom_yaw.velocity_ff + *input_.chassis_yaw_velocity_imu; + const double top_yaw_continuous_torque_ff = + target.top_yaw.acceleration_ff + top_yaw_viscous_ff_gain_ * target.top_yaw.velocity_ff; + const double bottom_yaw_torque_ff = + target.bottom_yaw.acceleration_ff + + friction_feedforward( + bottom_yaw_viscous_ff_gain_, bottom_yaw_coulomb_ff_gain_, + bottom_yaw_coulomb_ff_tanh_gain_, bottom_world_velocity_ff) + - k_top_to_bottom_ * top_yaw_continuous_torque_ff; + + const double top_yaw_torque_ff = target.top_yaw.acceleration_ff + + friction_feedforward( + top_yaw_viscous_ff_gain_, top_yaw_coulomb_ff_gain_, + top_yaw_coulomb_ff_tanh_gain_, top_velocity_ref); + const double pitch_torque_ff = + target.pitch.acceleration_ff + + friction_feedforward( + pitch_viscous_ff_gain_, pitch_coulomb_ff_gain_, pitch_coulomb_ff_tanh_gain_, + pitch_velocity_ref) + + pitch_gravity_ff_gain_ * std::sin(current_pitch_angle - pitch_gravity_ff_phase_); + + *output_.bottom_yaw_control_torque = + bottom_yaw_velocity_pid_.update(bottom_velocity_ref - current_bottom_velocity) + + bottom_yaw_torque_ff; + *output_.top_yaw_control_torque = + top_yaw_velocity_pid_.update(top_velocity_ref - *input_.top_yaw_velocity) + + top_yaw_torque_ff; + *output_.pitch_control_torque = + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity) + + pitch_torque_ff; + + *output_.yaw_control_angle_error = bottom_yaw_error; + } +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::EccentricDualYaw, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 4eba4ff4d..2951f53f8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -4,6 +4,10 @@ #include #include +#include +#include + +#include namespace rmcs_core::controller::pid { @@ -21,7 +25,7 @@ class PidCalculator { virtual ~PidCalculator() = default; void reset() { - last_err_ = nan; + last_err_ = nan; err_integral_ = 0; } @@ -57,4 +61,32 @@ class PidCalculator { double last_err_, err_integral_; }; -} // namespace rmcs_core::controller::pid \ No newline at end of file +inline auto make_pid_calculator( + rclcpp::Node& node, const std::string& prefix, // + std::optional kp_default = std::nullopt, + std::optional ki_default = std::nullopt, + std::optional kd_default = std::nullopt) { + + const auto parameter_or_default = + [&node](const std::string& name, std::optional default_value) { + if (default_value.has_value() && !node.has_parameter(name)) + node.declare_parameter(name, *default_value); + return node.get_parameter(name).as_double(); + }; + + auto calculator = PidCalculator{ + parameter_or_default(prefix + "kp", kp_default), + parameter_or_default(prefix + "ki", ki_default), + parameter_or_default(prefix + "kd", kd_default), + }; + + node.get_parameter(prefix + "integral_min", calculator.integral_min); + node.get_parameter(prefix + "integral_max", calculator.integral_max); + node.get_parameter(prefix + "integral_split_min", calculator.integral_split_min); + node.get_parameter(prefix + "integral_split_max", calculator.integral_split_max); + node.get_parameter(prefix + "output_min", calculator.output_min); + node.get_parameter(prefix + "output_max", calculator.output_max); + return calculator; +} + +} // namespace rmcs_core::controller::pid diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp index bb3f1c932..d1c56cbfd 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp @@ -200,6 +200,8 @@ class DjiMotor { return encoder_zero_point_; } + int last_raw_angle() const { return last_raw_angle_; } + double angle() const { return angle_; } double velocity() const { return velocity_; } double torque() const { return torque_; } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 68b2e4f65..c11061c23 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -203,6 +203,8 @@ class LkMotor { return encoder_zero_point_; } + int64_t last_raw_angle() const { return last_raw_angle_; } + double angle() const { return angle_; } double velocity() const { return velocity_; } double torque() const { return torque_; } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp new file mode 100644 index 000000000..9d917a2b1 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -0,0 +1,500 @@ +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +class Sentry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Sentry() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { + + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + + gimbal_board_ = + std::make_unique(get_parameter("board_serial_gimbal_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.08, 0.0, 0.0}); + tf_->set_transform( + Eigen::Translation3d{0.07128, 0.0, 0.0481}); + } + + auto update() -> void override { + top_board_->update(); + bottom_board_->update(); + gimbal_board_->update(); + tf_->set_transform( + gimbal_board_->imu_pose().conjugate()); + } + +private: + class GimbalBoard final : private librmcs::agent::CBoard { + public: + explicit GimbalBoard(std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) { + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); + } + + GimbalBoard(const GimbalBoard&) = delete; + GimbalBoard& operator=(const GimbalBoard&) = delete; + GimbalBoard(GimbalBoard&&) = delete; + GimbalBoard& operator=(GimbalBoard&&) = delete; + + ~GimbalBoard() override = default; + + auto update() -> void { + bmi088_.update_status(); + imu_pose_ = Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + } + + auto imu_pose() const -> Eigen::Quaterniond { return imu_pose_; } + + private: + auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) + -> void override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) + -> void override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + device::Bmi088 bmi088_{1000, 0.2, 0.0}; + Eigen::Quaterniond imu_pose_ = Eigen::Quaterniond::Identity(); + }; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + friend class Sentry; + + public: + explicit TopBoard( + Sentry& sentry, rmcs_executor::Component& sentry_command, + std::string_view board_serial = {}, librmcs::agent::AdvancedOptions options = {}) + : librmcs::agent::RmcsBoardLite(board_serial, options) + , tf_(sentry.tf_) + , bmi088_(1000, 0.2, 0.0) + , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") + , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") + , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") + , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") + , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reversed() + .set_reduction_ratio(19 * 2)); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + + sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); + } + + auto update() -> void { + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + + const auto pitch_angle = + std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi_v); + + bmi088_.update_status(); + const Eigen::Quaterniond gimbal_bmi088_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + gimbal_bmi088_pose.conjugate()); + + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); + + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + tf_->set_state( + gimbal_top_yaw_motor_.angle()); + tf_->set_state(pitch_angle); + } + + auto command_update() -> void { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); + } + + private: + auto can0_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x202) { + gimbal_left_friction_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } + + auto can3_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x141) + gimbal_top_yaw_motor_.store_status(data.can_data); + } + + auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) + -> void override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) + -> void override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + device::Bmi088 bmi088_; + device::LkMotor gimbal_pitch_motor_; + device::LkMotor gimbal_top_yaw_motor_; + device::DjiMotor gimbal_bullet_feeder_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + }; + + class BottomBoard final : private librmcs::agent::CBoard { + friend class Sentry; + + public: + explicit BottomBoard( + Sentry& sentry, rmcs_executor::Component& sentry_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , imu_(1000, 0.2, 0.0) + , tf_(sentry.tf_) + , dr16_(sentry) + , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") + , chassis_wheel_motors_( + {sentry, sentry_command, "/chassis/left_front_wheel"}, + {sentry, sentry_command, "/chassis/left_back_wheel"}, + {sentry, sentry_command, "/chassis/right_back_wheel"}, + {sentry, sentry_command, "/chassis/right_front_wheel"}) + , chassis_steer_motors_( + {sentry, sentry_command, "/chassis/left_front_steering"}, + {sentry, sentry_command, "/chassis/left_back_steering"}, + {sentry, sentry_command, "/chassis/right_back_steering"}, + {sentry, sentry_command, "/chassis/right_front_steering"}) + , supercap_(sentry, sentry_command) { + sentry.register_output("/referee/serial", referee_serial_); + + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point(static_cast(zero_point))); + + for (auto& motor : chassis_wheel_motors_) { + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(11.) + .enable_multi_turn_angle() + .set_reversed()); + } + + constexpr auto kSteerNames = std::array{ + "right_back_zero_point", + "right_front_zero_point", + "left_front_zero_point", + "left_back_zero_point", + }; + for (auto&& [motor, name] : std::views::zip(chassis_steer_motors_, kSteerNames)) { + const auto zero_point = sentry.get_parameter(name).as_int(); + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point(static_cast(zero_point)) + .enable_multi_turn_angle()); + } + + sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + } + + auto update() -> void { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + supercap_.update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steer_motors_) + motor.update_status(); + + dr16_.update_status(); + gimbal_bottom_yaw_motor_.update_status(); + tf_->set_state( + gimbal_bottom_yaw_motor_.angle()); + } + + auto command_update() -> void { + using namespace device; + + auto builder = start_transmit(); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + auto cache = CanPacket8{}; + auto generate = [&](std::uint32_t id, std::ranges::range auto& motors, auto... args) { + auto command = [&](T arg) { + if constexpr (std::same_as) { + return arg; + } else { + const auto valid = arg >= 0 && arg < 4; + return valid ? motors[arg].generate_command() + : CanPacket8::PaddingQuarter{}; + } + }; + cache = CanPacket8{command(args)...}; + return librmcs::data::CanDataView{.can_id = id, .can_data = cache.as_bytes()}; + }; + + if (can_transmission_mode_) { + builder.can1_transmit(generate(0x200, chassis_wheel_motors_, 1, 0, -1, -1)) + .can2_transmit(generate(0x200, chassis_wheel_motors_, -1, 2, -1, 3)); + } else { + builder.can1_transmit(generate(0x1FE, chassis_steer_motors_, 1, 0, -1, -1)) + .can2_transmit(generate( + 0x1FE, chassis_steer_motors_, 2, 3, -1, supercap_.generate_command())); + } + can_transmission_mode_ = !can_transmission_mode_; + } + + private: + auto dbus_receive_callback(const librmcs::data::UartDataView& data) -> void override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + auto can1_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x201) + chassis_wheel_motors_[1].store_status(data.can_data); + else if (can_id == 0x202) + chassis_wheel_motors_[0].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[1].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[0].store_status(data.can_data); + else if (can_id == 0x141) + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + + auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x202) + chassis_wheel_motors_[2].store_status(data.can_data); + else if (can_id == 0x204) + chassis_wheel_motors_[3].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[2].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[3].store_status(data.can_data); + else if (can_id == 0x300) + supercap_.store_status(data.can_data); + } + + auto uart1_receive_callback(const librmcs::data::UartDataView& data) -> void override { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); + } + + auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) + -> void override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) + -> void override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + bool can_transmission_mode_ = true; + device::Bmi088 imu_; + OutputInterface& tf_; + + device::Dr16 dr16_; + device::LkMotor gimbal_bottom_yaw_motor_; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_steer_motors_[4]; + device::Supercap supercap_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + OutputInterface chassis_yaw_velocity_imu_; + }; + + struct CommandTransmitter : public rmcs_executor::Component { + std::function fn; + + template + explicit CommandTransmitter(Fn&& fn) + : fn{std::forward(fn)} {} + + auto update() -> void override { fn(); } + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text("Gimbal Status"); + text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); + text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); + text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); + constexpr auto kPosition = + std::array{"right back", "right front", "left front", "left back"}; + constexpr auto kMaxLength = + std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); + + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_steer_motors_)) { + text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); + } + + response->message = feedback_message.str(); + } + + auto command_update() -> void { + top_board_->command_update(); + bottom_board_->command_update(); + } + std::shared_ptr command_component_{ + create_partner_component( + get_component_name() + "_command", [this] { command_update(); })}; + + InputInterface timestamp_; + OutputInterface tf_; + + std::unique_ptr gimbal_board_; + std::unique_ptr top_board_; + std::unique_ptr bottom_board_; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Sentry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp new file mode 100644 index 000000000..2fc9d17be --- /dev/null +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -0,0 +1,198 @@ +#pragma once + +#include + +#include + +#include + +namespace rmcs_description { + +struct BaseLink : fast_tf::Link { + static constexpr char name[] = "base_link"; +}; + +struct BottomYawLink : fast_tf::Link { + static constexpr char name[] = "bottom_yaw_link"; +}; +using YawLink = BottomYawLink; + +struct TopYawLink : fast_tf::Link { + static constexpr char name[] = "top_yaw_link"; +}; + +struct PitchLink : fast_tf::Link { + static constexpr char name[] = "pitch_link"; +}; + +struct MuzzleLink : fast_tf::Link { + static constexpr char name[] = "muzzle_link"; +}; + +struct CameraLink : fast_tf::Link { + static constexpr char name[] = "camera_link"; +}; + +struct TransmitterLink : fast_tf::Link { + static constexpr char name[] = "transmitter_link"; +}; + +struct OdomImu : fast_tf::Link { + static constexpr char name[] = "odom_imu"; +}; + +struct OdomGimbalImu : fast_tf::Link { + static constexpr char name[] = "odom_gimbal_imu"; +}; + +struct GimbalCenterLink : fast_tf::Link { + static constexpr char name[] = "gimbal_center_link"; +}; + +struct LeftFrontWheelLink : fast_tf::Link { + static constexpr char name[] = "left_front_wheel_link"; +}; +struct LeftBackWheelLink : fast_tf::Link { + static constexpr char name[] = "left_back_wheel_link"; +}; +struct RightBackWheelLink : fast_tf::Link { + static constexpr char name[] = "right_back_wheel_link"; +}; +struct RightFrontWheelLink : fast_tf::Link { + static constexpr char name[] = "right_front_wheel_link"; +}; + +} // namespace rmcs_description + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Translation3d transform = Eigen::Translation3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::GimbalCenterLink; + + void set_state(double angle) { angle_ = angle; } + auto get_transform() const { return Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitZ()}; } + +private: + double angle_ = 0.0; +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BottomYawLink; + + void set_transform(const Eigen::Translation3d& translation) { translation_ = translation; } + + void set_state(double angle) { angle_ = angle; } + + auto get_transform() const { + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + transform *= translation_; + transform *= Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitZ()}; + return transform; + } + +private: + Eigen::Translation3d translation_ = Eigen::Translation3d::Identity(); + double angle_ = 0.0; +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::TopYawLink; + + void set_state(double angle) { angle_ = angle; } + auto get_transform() const { return Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitY()}; } + +private: + double angle_ = 0.0; +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Translation3d transform = Eigen::Translation3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Translation3d transform = Eigen::Translation3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BottomYawLink; + Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +namespace rmcs_description { + +using Tf = fast_tf::JointCollection< + GimbalCenterLink, BottomYawLink, TopYawLink, PitchLink, MuzzleLink, TransmitterLink, CameraLink, + OdomImu, OdomGimbalImu, LeftFrontWheelLink, LeftBackWheelLink, RightBackWheelLink, + RightFrontWheelLink>; + +using SentryTf = Tf; + +} // namespace rmcs_description