diff --git a/include/rm_manual/balance_manual.h b/include/rm_manual/balance_manual.h index ca99c197..547e0a4e 100644 --- a/include/rm_manual/balance_manual.h +++ b/include/rm_manual/balance_manual.h @@ -1,5 +1,5 @@ // -// Created by yuchen on 2023/4/3. +// Created by kook on 9/28/24. // #pragma once @@ -15,6 +15,8 @@ class BalanceManual : public ChassisGimbalShooterCoverManual protected: void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; + void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; + void wPress() override; void sPress() override; void aPress() override; @@ -26,28 +28,13 @@ class BalanceManual : public ChassisGimbalShooterCoverManual void aPressing() override; void sPressing() override; void dPressing() override; - void bPress() override; - void vPress() override; void ctrlZPress() override; void rightSwitchDownRise() override; void rightSwitchMidRise() override; - void sendCommand(const ros::Time& time) override; - void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; - void ctrlXPress(); - void modeFallen(ros::Duration duration); - void modeNormalize(); - rm_common::BalanceCommandSender* balance_cmd_sender_{}; private: - void balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg); - - ros::Subscriber state_sub_; - double balance_dangerous_angle_; - bool flank_ = false, reverse_ = false; std::string flank_frame_, reverse_frame_; - - InputEvent v_event_, ctrl_x_event_, auto_fallen_event_; }; } // namespace rm_manual diff --git a/include/rm_manual/legged_wheel_balance_manual.h b/include/rm_manual/legged_wheel_balance_manual.h new file mode 100644 index 00000000..43d83029 --- /dev/null +++ b/include/rm_manual/legged_wheel_balance_manual.h @@ -0,0 +1,39 @@ +// +// Created by kook on 9/28/24. +// + +#pragma once + +#include "rm_manual/balance_manual.h" + +namespace rm_manual +{ +class LeggedWheelBalanceManual : public BalanceManual +{ +public: + LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); + +protected: + void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; + void shiftPress() override; + void shiftRelease() override; + void bPress() override; + void bRelease() override; + void ctrlZPress() override; + void rightSwitchDownRise() override; + void rightSwitchMidRise() override; + void ctrlShiftPress(); + void ctrlShiftRelease(); + + void sendCommand(const ros::Time& time) override; + void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; + void ctrlGPress(); + rm_common::LegCommandSender* legCommandSender_{}; + +private: + bool stretch_ = false, stretching_ = false, is_increasing_length_ = false; + InputEvent ctrl_shift_event_, ctrl_g_event_; + ros::Subscriber unstick_sub_; + void unstickCallback(const std_msgs::BoolConstPtr& msg); +}; +} // namespace rm_manual diff --git a/include/rm_manual/wheeled_balance_manual.h b/include/rm_manual/wheeled_balance_manual.h new file mode 100644 index 00000000..c3a04322 --- /dev/null +++ b/include/rm_manual/wheeled_balance_manual.h @@ -0,0 +1,37 @@ +// +// Created by yuchen on 2023/4/3. +// + +#pragma once + +#include "rm_manual/balance_manual.h" + +namespace rm_manual +{ +class WheeledBalanceManual : public BalanceManual +{ +public: + WheeledBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); + +protected: + void sendCommand(const ros::Time& time) override; + void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; + void rightSwitchDownRise() override; + void rightSwitchMidRise() override; + void ctrlZPress() override; + void bPress() override; + void vPress() override; + void ctrlXPress(); + void modeFallen(ros::Duration duration); + void modeNormalize(); + rm_common::BalanceCommandSender* balance_chassis_cmd_sender_{}; + +private: + void balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg); + + ros::Subscriber state_sub_; + double balance_dangerous_angle_; + + InputEvent ctrl_x_event_, auto_fallen_event_; +}; +} // namespace rm_manual diff --git a/src/balance_manual.cpp b/src/balance_manual.cpp index acd72e2a..386f2700 100644 --- a/src/balance_manual.cpp +++ b/src/balance_manual.cpp @@ -1,5 +1,5 @@ // -// Created by yuchen on 2023/4/3. +// Created by kook on 9/28/24. // #include "rm_manual/balance_manual.h" @@ -10,21 +10,9 @@ BalanceManual::BalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ChassisGimbalShooterCoverManual(nh, nh_referee) { ros::NodeHandle balance_nh(nh, "balance"); - balance_cmd_sender_ = new rm_common::BalanceCommandSender(balance_nh); - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); - - nh.param("flank_frame", flank_frame_, std::string("flank_frame")); - nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame")); - nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3); - + balance_nh.param("flank_frame", flank_frame_, std::string("flank_frame")); + balance_nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame")); is_balance_ = true; - state_sub_ = balance_nh.subscribe("/state", 1, &BalanceManual::balanceStateCallback, this); - x_event_.setRising(boost::bind(&BalanceManual::xPress, this)); - g_event_.setRising(boost::bind(&BalanceManual::gPress, this)); - v_event_.setRising(boost::bind(&BalanceManual::vPress, this)); - auto_fallen_event_.setActiveHigh(boost::bind(&BalanceManual::modeFallen, this, _1)); - auto_fallen_event_.setDelayTriggered(boost::bind(&BalanceManual::modeNormalize, this), 1.5, true); - ctrl_x_event_.setRising(boost::bind(&BalanceManual::ctrlXPress, this)); } void BalanceManual::sendCommand(const ros::Time& time) @@ -49,14 +37,11 @@ void BalanceManual::sendCommand(const ros::Time& time) ChassisGimbalShooterManual::sendCommand(time); cover_command_sender_->sendCommand(time); - balance_cmd_sender_->sendCommand(time); } void BalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalShooterCoverManual::checkKeyboard(dbus_data); - v_event_.update(dbus_data->key_v && !dbus_data->key_ctrl); - ctrl_x_event_.update(dbus_data->key_ctrl && dbus_data->key_x); } void BalanceManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) @@ -72,7 +57,6 @@ void BalanceManual::rightSwitchDownRise() { ChassisGimbalShooterCoverManual::rightSwitchDownRise(); state_ = RC; - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FALLEN); chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); } @@ -80,7 +64,6 @@ void BalanceManual::rightSwitchDownRise() void BalanceManual::rightSwitchMidRise() { ChassisGimbalShooterCoverManual::rightSwitchMidRise(); - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); } @@ -90,13 +73,11 @@ void BalanceManual::ctrlZPress() ChassisGimbalShooterCoverManual::ctrlZPress(); if (supply_) { - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FALLEN); chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); } else { - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); } @@ -111,18 +92,7 @@ void BalanceManual::shiftPress() { ChassisGimbalShooterCoverManual::shiftPress(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::UP_SLOPE); - chassis_cmd_sender_->updateSafetyPower(60); -} - -void BalanceManual::vPress() -{ - chassis_cmd_sender_->updateSafetyPower(80); -} - -void BalanceManual::bPress() -{ - ChassisGimbalShooterCoverManual::bPress(); - chassis_cmd_sender_->updateSafetyPower(100); + chassis_cmd_sender_->updateSafetyPower(220); } void BalanceManual::wPress() @@ -197,54 +167,13 @@ void BalanceManual::cPress() { if (is_gyro_) { - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); - vel_cmd_sender_->setAngularZVel(0.0); - is_gyro_ = false; + setChassisMode(rm_msgs::ChassisCmd::FOLLOW); } else { - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - is_gyro_ = true; - if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); - else - vel_cmd_sender_->setAngularZVel(1.0); + setChassisMode(rm_msgs::ChassisCmd::RAW); + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); } } -void BalanceManual::ctrlXPress() -{ - if (balance_cmd_sender_->getMsg()->data == rm_msgs::BalanceState::NORMAL) - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); - else - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); -} - -void BalanceManual::balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg) -{ - if ((ros::Time::now() - msg->header.stamp).toSec() < 0.2) - { - if (std::abs(msg->theta) > balance_dangerous_angle_) - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); - if (msg->mode == rm_msgs::BalanceState::NORMAL) - auto_fallen_event_.update(std::abs(msg->theta) > 0.42 && std::abs(msg->x_dot) > 1.5 && - vel_cmd_sender_->getMsg()->linear.x == 0 && vel_cmd_sender_->getMsg()->linear.y == 0 && - vel_cmd_sender_->getMsg()->angular.z == 0); - } -} - -void BalanceManual::modeNormalize() -{ - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); - ROS_INFO("mode normalize"); -} - -void BalanceManual::modeFallen(ros::Duration duration) -{ - if (duration.toSec() > 0.3) - { - balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); - ROS_INFO("mode fallen"); - } -} } // namespace rm_manual diff --git a/src/legged_wheel_balance_manual.cpp b/src/legged_wheel_balance_manual.cpp new file mode 100644 index 00000000..976b3677 --- /dev/null +++ b/src/legged_wheel_balance_manual.cpp @@ -0,0 +1,167 @@ +// +// Created by kook on 9/28/24. +// + +#include "rm_manual/legged_wheel_balance_manual.h" + +namespace rm_manual +{ +LeggedWheelBalanceManual::LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) + : BalanceManual(nh, nh_referee) +{ + ros::NodeHandle leg_wheel_chassis_nh(nh, "balance/legged_wheel_chassis"); + legCommandSender_ = new rm_common::LegCommandSender(leg_wheel_chassis_nh); + legCommandSender_->setLgeLength(0.18); + legCommandSender_->setJump(false); + + b_event_.setEdge(boost::bind(&LeggedWheelBalanceManual::bPress, this), + boost::bind(&LeggedWheelBalanceManual::bRelease, this)); + ctrl_shift_event_.setEdge(boost::bind(&LeggedWheelBalanceManual::ctrlShiftPress, this), + boost::bind(&LeggedWheelBalanceManual::ctrlShiftRelease, this)); + ctrl_g_event_.setRising(boost::bind(&LeggedWheelBalanceManual::ctrlGPress, this)); + + std::string unstick_topic; + leg_wheel_chassis_nh.param("unstick_topic", unstick_topic, + std::string("/controllers/legged_balance_controller/unstick/two_leg_unstick")); + unstick_sub_ = leg_wheel_chassis_nh.subscribe(unstick_topic, 1, + &LeggedWheelBalanceManual::unstickCallback, this); +} + +void LeggedWheelBalanceManual::sendCommand(const ros::Time& time) +{ + BalanceManual::sendCommand(time); + if (is_gyro_) + { + double current_length = legCommandSender_->getLgeLength(); + if (is_increasing_length_) + { + if (current_length < 0.3) + { + double delta = current_length + 0.002; + legCommandSender_->setLgeLength(delta > 0.3 ? 0.3 : delta); + } + else + is_increasing_length_ = false; + } + else + { + if (current_length > 0.18) + { + double delta = current_length - 0.002; + legCommandSender_->setLgeLength(delta < 0.18 ? 0.18 : delta); + } + else + is_increasing_length_ = true; + } + } + legCommandSender_->sendCommand(time); +} + +void LeggedWheelBalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ + ChassisGimbalShooterCoverManual::checkKeyboard(dbus_data); + ctrl_g_event_.update(dbus_data->key_ctrl && dbus_data->key_g); + ctrl_shift_event_.update(dbus_data->key_ctrl && dbus_data->key_shift); +} + +void LeggedWheelBalanceManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ + BalanceManual::updateRc(dbus_data); + if (!is_gyro_) + { // Capacitor enter fast charge when chassis stop. + if (!dbus_data->wheel && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW && + std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) > + 0.0) + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW) + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + } + else + { + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + } +} + +void LeggedWheelBalanceManual::rightSwitchDownRise() +{ + BalanceManual::rightSwitchDownRise(); +} + +void LeggedWheelBalanceManual::rightSwitchMidRise() +{ + BalanceManual::rightSwitchMidRise(); +} + +void LeggedWheelBalanceManual::ctrlZPress() +{ + BalanceManual::ctrlZPress(); + if (!supply_) + { + setChassisMode(rm_msgs::ChassisCmd::FOLLOW); + legCommandSender_->setLgeLength(0.18); + } +} + +void LeggedWheelBalanceManual::shiftRelease() +{ + BalanceManual::shiftRelease(); +} + +void LeggedWheelBalanceManual::shiftPress() +{ + BalanceManual::shiftPress(); +} + +void LeggedWheelBalanceManual::ctrlShiftPress() +{ + legCommandSender_->setJump(true); +} + +void LeggedWheelBalanceManual::ctrlShiftRelease() +{ + legCommandSender_->setJump(false); +} + +void LeggedWheelBalanceManual::bPress() +{ + ChassisGimbalShooterCoverManual::bPress(); + chassis_cmd_sender_->updateSafetyPower(60); +} + +void LeggedWheelBalanceManual::bRelease() +{ + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + chassis_cmd_sender_->updateSafetyPower(60); +} + +void LeggedWheelBalanceManual::ctrlGPress() +{ + if (!stretch_) + { + legCommandSender_->setLgeLength(0.3); + stretch_ = true; + } + else + { + legCommandSender_->setLgeLength(0.18); + stretch_ = false; + } +} + +void LeggedWheelBalanceManual::unstickCallback(const std_msgs::BoolConstPtr& msg) +{ + auto two_leg_unstick = msg->data; + if (two_leg_unstick) + { + auto delta = legCommandSender_->getLgeLength() + 0.02; + legCommandSender_->setLgeLength(delta > 0.33 ? 0.33 : delta); + stretching_ = true; + } + else if (stretching_ && !two_leg_unstick) + { + legCommandSender_->setLgeLength(0.18); + stretching_ = false; + } +} + +} // namespace rm_manual diff --git a/src/main.cpp b/src/main.cpp index b515ec08..f813ec22 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,7 +6,8 @@ #include "rm_manual/chassis_gimbal_shooter_cover_manual.h" #include "rm_manual/engineer_manual.h" #include "rm_manual/dart_manual.h" -#include "rm_manual/balance_manual.h" +#include "rm_manual/wheeled_balance_manual.h" +#include "rm_manual/legged_wheel_balance_manual.h" int main(int argc, char** argv) { @@ -24,8 +25,10 @@ int main(int argc, char** argv) manual_control = new rm_manual::EngineerManual(nh, nh_referee); else if (robot == "dart") manual_control = new rm_manual::DartManual(nh, nh_referee); - else if (robot == "balance") - manual_control = new rm_manual::BalanceManual(nh, nh_referee); + else if (robot == "wheeled_balance") + manual_control = new rm_manual::WheeledBalanceManual(nh, nh_referee); + else if (robot == "legged_wheel_balance") + manual_control = new rm_manual::LeggedWheelBalanceManual(nh, nh_referee); else { ROS_ERROR("no robot type "); diff --git a/src/wheeled_balance_manual.cpp b/src/wheeled_balance_manual.cpp new file mode 100644 index 00000000..286952ef --- /dev/null +++ b/src/wheeled_balance_manual.cpp @@ -0,0 +1,111 @@ +// +// Created by yuchen on 2023/4/3. +// + +#include "rm_manual/wheeled_balance_manual.h" + +namespace rm_manual +{ +WheeledBalanceManual::WheeledBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) + : BalanceManual(nh, nh_referee) +{ + ros::NodeHandle wheeled_chassis_nh(nh, "balance/wheeled_chassis"); + balance_chassis_cmd_sender_ = new rm_common::BalanceCommandSender(wheeled_chassis_nh); + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); + + wheeled_chassis_nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3); + + state_sub_ = wheeled_chassis_nh.subscribe("/state", 1, + &WheeledBalanceManual::balanceStateCallback, this); + x_event_.setRising(boost::bind(&WheeledBalanceManual::xPress, this)); + v_event_.setRising(boost::bind(&WheeledBalanceManual::vPress, this)); + auto_fallen_event_.setActiveHigh(boost::bind(&WheeledBalanceManual::modeFallen, this, _1)); + auto_fallen_event_.setDelayTriggered(boost::bind(&WheeledBalanceManual::modeNormalize, this), 1.5, true); + ctrl_x_event_.setRising(boost::bind(&WheeledBalanceManual::ctrlXPress, this)); +} + +void WheeledBalanceManual::sendCommand(const ros::Time& time) +{ + BalanceManual::sendCommand(time); + balance_chassis_cmd_sender_->sendCommand(time); +} + +void WheeledBalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ + ChassisGimbalShooterCoverManual::checkKeyboard(dbus_data); + v_event_.update(dbus_data->key_v && !dbus_data->key_ctrl); + ctrl_x_event_.update(dbus_data->key_ctrl && dbus_data->key_x); +} + +void WheeledBalanceManual::rightSwitchDownRise() +{ + BalanceManual::rightSwitchDownRise(); + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); +} + +void WheeledBalanceManual::rightSwitchMidRise() +{ + BalanceManual::rightSwitchMidRise(); + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); +} + +void WheeledBalanceManual::ctrlZPress() +{ + BalanceManual::ctrlZPress(); + if (supply_) + { + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); + } + else + { + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); + } +} + +void WheeledBalanceManual::vPress() +{ + chassis_cmd_sender_->updateSafetyPower(80); +} + +void WheeledBalanceManual::bPress() +{ + ChassisGimbalShooterCoverManual::bPress(); + chassis_cmd_sender_->updateSafetyPower(100); +} + +void WheeledBalanceManual::ctrlXPress() +{ + if (balance_chassis_cmd_sender_->getMsg()->data == rm_msgs::BalanceState::NORMAL) + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); + else + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); +} + +void WheeledBalanceManual::balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg) +{ + if ((ros::Time::now() - msg->header.stamp).toSec() < 0.2) + { + if (std::abs(msg->theta) > balance_dangerous_angle_) + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); + if (msg->mode == rm_msgs::BalanceState::NORMAL) + auto_fallen_event_.update(std::abs(msg->theta) > 0.42 && std::abs(msg->x_dot) > 1.5 && + vel_cmd_sender_->getMsg()->linear.x == 0 && vel_cmd_sender_->getMsg()->linear.y == 0 && + vel_cmd_sender_->getMsg()->angular.z == 0); + } +} + +void WheeledBalanceManual::modeNormalize() +{ + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL); + ROS_INFO("mode normalize"); +} + +void WheeledBalanceManual::modeFallen(ros::Duration duration) +{ + if (duration.toSec() > 0.3) + { + balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN); + ROS_INFO("mode fallen"); + } +} +} // namespace rm_manual