diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index a61205e2..d6ffb1a7 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(Eigen3 REQUIRED) generate_dynamic_reconfigure_options( cfg/LQRWeight.cfg - cfg/PowerLimit.cfg ) ################################### diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index 0b6a81ae..800750bf 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -119,6 +119,10 @@ sudo rosdep install --from-paths src Allowed period (in s) between two commands. If the time is exceed this period, the speed of chassis will be set 0. +* **`raw_yaw_feedforward_k`** (double, default: 0.0) + + Yaw feedforward time constant in seconds. Only used in `raw` mode, where the command-frame rotation uses `yaw + k * w` to compensate direction-change lag. + * **`power_offset`** (double) Fix the difference between theoretical power and actual power. @@ -245,6 +249,7 @@ sudo rosdep install --from-paths src power_offset: -8.41 twist_angular: 0.5233 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg deleted file mode 100644 index 52247c4d..00000000 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python3 -PACKAGE = "rm_chassis_controllers" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.000, 0.000, 1.000) -gen.add("effort_coeff", double_t, 0, "effort coefficient for power limit", 0.0, 0.0, 20.0) -gen.add("power_offset", double_t, 0, "power offset for power limit", 0.0, -50.0, 50.0) - -exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 9a95c74c..099adc20 100755 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -153,11 +152,12 @@ class ChassisBase : public controller_interface::MultiInterfaceController /** @brief Set chassis velocity to zero. */ void recovery(); + void fallen(); /** @brief Transform tf velocity to base link frame. * * @param from The father frame. */ - void tfVelToBase(const std::string& from); + void tfVelToBase(const std::string& from, double yaw_offset = 0.); /** @brief To limit the chassis power according to current power limit. * * Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit. @@ -182,7 +182,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; - std::vector joint_handles_{}; + std::vector wheel_joint_handles_{}; realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; realtime_tools::RealtimeBuffer slam_rt_buffer_{}; realtime_tools::RealtimeBuffer localization_rt_buffer_{}; @@ -228,6 +228,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController double twist_angular_{ M_PI / 6 }; double max_odom_vel_{ 10.0 }; double timeout_{ 0.1 }; + double raw_yaw_feedforward_k_{ 0.0 }; double chassis_power_{ 0.0 }; bool capacity_update_flag_{ false }; bool use_rls_{ false }; @@ -260,9 +261,10 @@ class ChassisBase : public controller_interface::MultiInterfaceController enum { - RAW, - FOLLOW, - TWIST + RAW = 0, + FOLLOW = 1, + TWIST = 2, + FALLEN = 4 }; }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 1bc67336..8c482f03 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include "rm_chassis_controllers/chassis_base.h" @@ -16,14 +18,20 @@ class OmniController : public ChassisBase> joints_; Eigen::MatrixXd chassis2joints_; + void powerLimit() override; + void updatePowerStatus() override; + virtual void stateJudge(); -protected: - void moveJoint(const ros::Time& time, const ros::Duration& period) override; + std::array motor_lp_filters_{}; + std::unique_ptr> rls_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 23ddfb94..bdf775b3 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -39,8 +39,15 @@ #include "rm_chassis_controllers/chassis_base.h" +#include +#include #include #include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { @@ -52,7 +59,8 @@ struct Module effort_controllers::JointVelocityController* ctrl_wheel_; }; -class SwerveController : public ChassisBase +class SwerveController : public ChassisBase { public: SwerveController() = default; @@ -61,7 +69,18 @@ class SwerveController : public ChassisBase modules_; + void powerLimit() override; + void updatePowerStatus() override; + void getBaseGyro(); + void stateJudge(); + + std::vector modules_{}; + std::vector pivot_joint_handles_{}; + std::unique_ptr> base_gyro_pub_; + + PowerLimitor pivot_power_limitor_{}; + std::array, 2> motor_lp_filters_{}; + std::unique_ptr> rls_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 93b9bb73..dc5f0d7c 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -32,8 +32,8 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan } left_wheel_joint_handle_ = robot_hw->get()->getHandle(left_wheel_joint); right_wheel_joint_handle_ = robot_hw->get()->getHandle(right_wheel_joint); - joint_handles_.push_back(left_wheel_joint_handle_); - joint_handles_.push_back(right_wheel_joint_handle_); + wheel_joint_handles_.push_back(left_wheel_joint_handle_); + wheel_joint_handles_.push_back(right_wheel_joint_handle_); left_momentum_block_joint_handle_ = robot_hw->get()->getHandle(left_momentum_block_joint); right_momentum_block_joint_handle_ = diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index f1c13625..69d531c0 100755 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -57,6 +57,7 @@ void ChassisBase::initialize_parameters(ros::NodeHandle& controller_nh) controller_nh.getParam("twist_angular", twist_angular_); controller_nh.getParam("max_odom_vel", max_odom_vel_); controller_nh.getParam("timeout", timeout_); + raw_yaw_feedforward_k_ = getParam(controller_nh, "raw_yaw_feedforward_k", 0.0); if (controller_nh.hasParam("pid_follow")) pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow")); @@ -200,6 +201,9 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio case TWIST: twist(time, period); break; + case FALLEN: + fallen(); + break; } ramp_w_->setAcc(cmd_chassis.accel.angular.z); @@ -290,7 +294,29 @@ void ChassisBase::raw() recovery(); } - tfVelToBase(command_source_frame_); + double yaw_offset; + if (command_source_frame_ == "yaw") + yaw_offset = raw_yaw_feedforward_k_ * vel_cmd_.z; + else + yaw_offset = 0.; + tfVelToBase(command_source_frame_, yaw_offset); +} + +template +void ChassisBase::fallen() +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Chassis] Enter FALLEN"); + } + + ramp_x_->clear(); + ramp_y_->clear(); + ramp_w_->clear(); + vel_cmd_.x = 0.; + vel_cmd_.y = 0.; + vel_cmd_.z = 0.; } template @@ -474,11 +500,22 @@ void ChassisBase::updatePowerStatus() } template -void ChassisBase::tfVelToBase(const std::string& from) +void ChassisBase::tfVelToBase(const std::string& from, double yaw_offset) { try { - tf2::doTransform(vel_cmd_, vel_cmd_, robot_state_handle_.lookupTransform("base_link", from, ros::Time(0))); + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("base_link", from, ros::Time(0)); + if (std::abs(yaw_offset) > 1e-9) + { + tf2::Quaternion rotation; + tf2::fromMsg(transform.transform.rotation, rotation); + tf2::Quaternion yaw_feedforward; + yaw_feedforward.setRPY(0., 0., yaw_offset); + rotation *= yaw_feedforward; + rotation.normalize(); + transform.transform.rotation = tf2::toMsg(rotation); + } + tf2::doTransform(vel_cmd_, vel_cmd_, transform); } catch (tf2::TransformException& ex) { diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 903ad007..2582cc1f 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -2,6 +2,8 @@ // Created by qiayuan on 2022/7/29. // +#include +#include #include #include @@ -17,6 +19,41 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle { ChassisBase::init(robot_hw, root_nh, controller_nh); + // Set init value for RLS and power limiters. + try + { + controller_nh.getParam("power/vel_coeff", wheel_power_limitor_.vel_coeff); + controller_nh.getParam("power/effort_coeff", wheel_power_limitor_.effort_coeff); + controller_nh.getParam("power/power_offset", wheel_power_limitor_.power_offset); + controller_nh.param("power/use_rls", use_rls_, false); + controller_nh.param("power/use_K_angle", use_K_angle_, false); + } + catch (const std::exception& e) + { + ROS_ERROR("Failed to get power limiter parameters: %s", e.what()); + return false; + } + + wheel_power_limitor_.err_upper = 500; + wheel_power_limitor_.err_lower = 0.01; + + rls_ = std::make_unique>(2, 1, 0.99999, 1e-5); + Eigen::Matrix w; + w << wheel_power_limitor_.effort_coeff, wheel_power_limitor_.vel_coeff; + rls_->setW(w); + + for (auto& filter : motor_lp_filters_) + { + filter = new LowPassFilter(20); + } + + auto epower_publisher = + std::make_unique>(controller_nh, "power/estimated", 100); + this->epower_pub_ = std::move(epower_publisher); + auto cpower_publisher = + std::make_unique>(controller_nh, "power/commanded", 100); + this->cpower_pub_ = std::move(cpower_publisher); + XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); chassis2joints_.resize(wheels.size(), 3); @@ -44,7 +81,7 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle joints_.push_back(std::make_shared()); if (!joints_.back()->init(effort_joint_interface_, nh_wheel)) return false; - joint_handles_.push_back(joints_[i]->joint_); + wheel_joint_handles_.push_back(joints_[i]->joint_); i++; } @@ -77,5 +114,188 @@ geometry_msgs::Twist OmniController::odometry() return twist; } +void OmniController::stateJudge() +{ + if (!use_K_angle_) + { + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + wheel_power_limitor_.K_angle[i] = 1.0; + } + return; + } + double sin_pitch{}; + if (abs(pitch_) > 0.12) + { + sin_pitch = sin(pitch_); + } + else + { + sin_pitch = 0.0; + } + + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + + if (ctl->joint_.getName().find("front") != std::string::npos) + { + if (ctl->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + if (ctl->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + } + if (ctl->joint_.getName().find("back") != std::string::npos) + { + if (ctl->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + if (ctl->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + } + } +} + +void OmniController::powerLimit() +{ + updatePowerStatus(); + stateJudge(); + // multiply K to limit power for wheel joints. + if (wheel_power_limitor_.err_sum > wheel_power_limitor_.err_upper) + { + wheel_power_limitor_.K = 1; + } + else if (wheel_power_limitor_.err_sum < wheel_power_limitor_.err_lower) + { + wheel_power_limitor_.K = 0; + } + else + { + wheel_power_limitor_.K = 1 - (wheel_power_limitor_.err_sum - wheel_power_limitor_.err_lower) / + (wheel_power_limitor_.err_upper - wheel_power_limitor_.err_lower); + } + // Set power limit to each joint according to K. + for (int i = 0; i < 4; i++) + { + double wheel_zoom = + (wheel_power_limitor_.K * abs(wheel_power_limitor_.err[i]) / wheel_power_limitor_.err_sum) + + (1 - wheel_power_limitor_.K) * (abs(wheel_power_limitor_.power_in[i]) / wheel_power_limitor_.cmd_power); + wheel_zoom = limit(wheel_zoom, 0.0, 1.0); + wheel_power_limitor_.power_limit[i] = wheel_zoom * wheel_power_limitor_.max_power; + } + if (wheel_power_limitor_.cmd_power > wheel_power_limitor_.max_power) + { + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + auto& joint = ctl->joint_; + double A = wheel_power_limitor_.effort_coeff; + double B = wheel_power_limitor_.omiga[i]; + double C = abs(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + wheel_power_limitor_.power_offset / 4 - wheel_power_limitor_.power_limit[i]; + double Delta = square(B) - 4 * A * C; + if (!std::isfinite(Delta) || Delta < 0.0) + Delta = 0.0; + if (Delta >= 0) + { + double Sqrt = sqrtf(Delta); + if (wheel_power_limitor_.torque[i] >= 0) + joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + else + joint.setCommand(((-B - Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + else + { + joint.setCommand(((-B) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + } + } + else + { + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + auto& joint = ctl->joint_; + joint.setCommand(wheel_power_limitor_.torque[i] * wheel_power_limitor_.K_angle[i]); + } + } +} + +void OmniController::updatePowerStatus() +{ + double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + + wheel_power_limitor_.max_power = power_limit; + wheel_power_limitor_.err_sum = 0; + + double ewheel_power{}, cwheel_power{}; + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + + double cmd_torque = wheel_power_limitor_.torque[i] = ctl->joint_.getCommand(); + double cmd_vel{}; + ctl->getCommand(cmd_vel); + double real_vel = wheel_power_limitor_.omiga[i] = ctl->joint_.getVelocity(); + motor_lp_filters_[i]->input(ctl->joint_.getEffort()); + double real_torque = motor_lp_filters_[i]->output(); + + wheel_power_limitor_.err[i] = cmd_vel - real_vel; + wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); + + ewheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + + wheel_power_limitor_.power_in[i] = cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + } + wheel_power_limitor_.cmd_power = cwheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.estimated_power = ewheel_power + wheel_power_limitor_.power_offset; + + double estimated_total_power = wheel_power_limitor_.estimated_power; + double cmd_total_power = wheel_power_limitor_.cmd_power; + + if (capacity_update_flag_ && use_rls_ && estimated_total_power > 0) + { + // Update Rls. + double all_in = limit(estimated_total_power, -power_limit, power_limit); + // Update Rls: compute regression vector x and pass actual measured power + Eigen::Matrix x; + x(0) = square(wheel_power_limitor_.torque[0]) + square(wheel_power_limitor_.torque[1]) + + square(wheel_power_limitor_.torque[2]) + square(wheel_power_limitor_.torque[3]); + x(1) = abs(wheel_power_limitor_.omiga[0]) + abs(wheel_power_limitor_.omiga[1]) + + abs(wheel_power_limitor_.omiga[2]) + abs(wheel_power_limitor_.omiga[3]); + rls_->setU(all_in); + rls_->setX(x); + rls_->setY(chassis_power_); + rls_->update(); + auto w = rls_->getW(); + wheel_power_limitor_.effort_coeff = std::max(w(0), 1e-3); + wheel_power_limitor_.vel_coeff = std::max(w(1), 1e-3); + capacity_update_flag_ = false; + } + + // Publish power status. + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(epower_pub_, estimated_total_power); + publishPower(cpower_pub_, cmd_total_power); +} + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::OmniController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/sentry.cpp b/rm_chassis_controllers/src/sentry.cpp index e26ad8d3..ce5e0fb7 100644 --- a/rm_chassis_controllers/src/sentry.cpp +++ b/rm_chassis_controllers/src/sentry.cpp @@ -56,8 +56,8 @@ bool SentryController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand !ctrl_catapult_joint_.init(effort_joint_interface_, nh_brake)) return false; if_catapult_ = false; - joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_wheel_.getJointName())); - joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_catapult_joint_.getJointName())); + wheel_joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_wheel_.getJointName())); + wheel_joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_catapult_joint_.getJointName())); return true; } diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 69c21077..9f54947e 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -36,7 +36,7 @@ // #include "rm_chassis_controllers/swerve.h" - +#include "rm_common/math_utilities.h" #include #include @@ -47,6 +47,55 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand { if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) return false; + + // Set init value for RLS and power limiters. + try + { + controller_nh.getParam("power/vel_coeff", wheel_power_limitor_.vel_coeff); + controller_nh.getParam("power/effort_coeff", wheel_power_limitor_.effort_coeff); + controller_nh.getParam("power/power_offset", wheel_power_limitor_.power_offset); + controller_nh.getParam("power/pivot_vel_coeff", pivot_power_limitor_.vel_coeff); + controller_nh.getParam("power/pivot_effort_coeff", pivot_power_limitor_.effort_coeff); + controller_nh.getParam("power/pivot_power_offset", pivot_power_limitor_.power_offset); + controller_nh.getParam("power/pivot_power_ratio", pivot_power_limitor_.ratio); + controller_nh.param("power/use_rls", use_rls_, false); + controller_nh.param("power/use_K_angle", use_K_angle_, false); + } + catch (const std::exception& e) + { + ROS_ERROR("Failed to get power limiter parameters: %s", e.what()); + return false; + } + + // pivot don't need err to multiply power. + wheel_power_limitor_.err_upper = 500; + wheel_power_limitor_.err_lower = 0.01; + + rls_ = std::make_unique>(4, 1, 0.99999, 1e-5); + Eigen::Matrix w; + w << pivot_power_limitor_.effort_coeff, pivot_power_limitor_.vel_coeff, wheel_power_limitor_.effort_coeff, + wheel_power_limitor_.vel_coeff; + rls_->setW(w); + + // 0 for pivot, 1 for wheel. Each module has 4 joints at most. + for (auto& filter_group : motor_lp_filters_) + { + for (auto& filter : filter_group) + filter = new LowPassFilter(20); + } + + // Setup power publishers. + auto epower_publisher = + std::make_unique>(controller_nh, "power/estimated", 100); + this->epower_pub_ = std::move(epower_publisher); + auto cpower_publisher = + std::make_unique>(controller_nh, "power/commanded", 100); + this->cpower_pub_ = std::move(cpower_publisher); + // Todo :add base_imu and base_gyro publishers for better power estimation and detect state. + auto base_gyro_publisher = std::make_unique>( + controller_nh, "base_gyro", 100); + this->base_gyro_pub_ = std::move(base_gyro_publisher); + XmlRpc::XmlRpcValue modules; controller_nh.getParam("modules", modules); ROS_ASSERT(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct); @@ -73,10 +122,11 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand return false; if (module.second["pivot"].hasMember("offset")) m.pivot_offset_ = module.second["pivot"]["offset"]; - joint_handles_.push_back(m.ctrl_pivot_->joint_); - joint_handles_.push_back(m.ctrl_wheel_->joint_); + pivot_joint_handles_.push_back(m.ctrl_pivot_->joint_); + wheel_joint_handles_.push_back(m.ctrl_wheel_->joint_); modules_.push_back(m); } + return true; } @@ -84,12 +134,12 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& period) { + stateJudge(); Vec2 vel_center(vel_cmd_.x, vel_cmd_.y); for (auto& module : modules_) { Vec2 vel = vel_center + vel_cmd_.z * Vec2(-module.position_.y(), module.position_.x()); double vel_angle = std::atan2(vel.y(), vel.x()) + module.pivot_offset_; - // Direction flipping and Stray module mitigation double a = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle); double b = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle + M_PI); module.ctrl_pivot_->setCommand(std::abs(a) < std::abs(b) ? vel_angle : vel_angle + M_PI); @@ -125,5 +175,252 @@ geometry_msgs::Twist SwerveController::odometry() return vel_data; } +void SwerveController::stateJudge() +{ + if (!use_K_angle_) + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + wheel_power_limitor_.K_angle[i] = 1.0; + } + return; + } + double sin_pitch{}; + if (abs(pitch_) > 0.12) + { + sin_pitch = sin(pitch_); + } + else + { + sin_pitch = 0.0; + } + + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + + if (module.ctrl_wheel_->joint_.getName().find("front") != std::string::npos) + { + if (module.ctrl_wheel_->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + if (module.ctrl_wheel_->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + } + if (module.ctrl_wheel_->joint_.getName().find("back") != std::string::npos) + { + if (module.ctrl_wheel_->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + if (module.ctrl_wheel_->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + } + } +} + +// Ref: https://gitee.com/cod_-control/rmcod2026_-sentry/tree/dev +// https://github.com/hkustenterprize/RM2024-PowerModule + +void SwerveController::powerLimit() +{ + updatePowerStatus(); + + // multiply K to limit power for wheel joints. + if (wheel_power_limitor_.err_sum > wheel_power_limitor_.err_upper) + { + wheel_power_limitor_.K = 1; + } + else if (wheel_power_limitor_.err_sum < wheel_power_limitor_.err_lower) + { + wheel_power_limitor_.K = 0; + } + else + { + wheel_power_limitor_.K = 1 - (wheel_power_limitor_.err_sum - wheel_power_limitor_.err_lower) / + (wheel_power_limitor_.err_upper - wheel_power_limitor_.err_lower); + } + // Set power limit to each joint according to K. + for (int i = 0; i < 4; i++) + { + double pivot_zoom = abs(pivot_power_limitor_.power_in[i]) / pivot_power_limitor_.cmd_power; + pivot_zoom = limit(pivot_zoom, 0.0, 1.0); + double wheel_zoom = + (wheel_power_limitor_.K * abs(wheel_power_limitor_.err[i]) / wheel_power_limitor_.err_sum) + + (1 - wheel_power_limitor_.K) * (abs(wheel_power_limitor_.power_in[i]) / wheel_power_limitor_.cmd_power); + wheel_zoom = limit(wheel_zoom, 0.0, 1.0); + pivot_power_limitor_.power_limit[i] = pivot_zoom * pivot_power_limitor_.max_power; + wheel_power_limitor_.power_limit[i] = wheel_zoom * wheel_power_limitor_.max_power; + } + // Set command limit according to power limit. + if (pivot_power_limitor_.cmd_power > pivot_power_limitor_.max_power) + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + auto& joint = module.ctrl_pivot_->joint_; + double A = pivot_power_limitor_.effort_coeff; + double B = pivot_power_limitor_.omiga[i]; + double C = abs(pivot_power_limitor_.omiga[i]) * pivot_power_limitor_.vel_coeff + + pivot_power_limitor_.power_offset / 4 - pivot_power_limitor_.power_limit[i]; + double Delta = square(B) - 4 * A * C; + if (!std::isfinite(Delta) || Delta < 0.0) + Delta = 0.0; + if (Delta >= 0) + { + double Sqrt = sqrtf(Delta); + if (pivot_power_limitor_.torque[i] >= 0) + joint.setCommand((-B + Sqrt) / (2 * A)); + else + joint.setCommand((-B - Sqrt) / (2 * A)); + } + else + { + joint.setCommand((-B) / (2 * A)); + } + } + } + if (wheel_power_limitor_.cmd_power > wheel_power_limitor_.max_power) + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + auto& joint = module.ctrl_wheel_->joint_; + double A = wheel_power_limitor_.effort_coeff; + double B = wheel_power_limitor_.omiga[i]; + double C = abs(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + wheel_power_limitor_.power_offset / 4 - wheel_power_limitor_.power_limit[i]; + double Delta = square(B) - 4 * A * C; + if (!std::isfinite(Delta) || Delta < 0.0) + Delta = 0.0; + if (Delta >= 0) + { + double Sqrt = sqrtf(Delta); + if (wheel_power_limitor_.torque[i] >= 0) + joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + else + joint.setCommand(((-B - Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + else + { + joint.setCommand(((-B) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + } + } + else + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + auto& joint = module.ctrl_wheel_->joint_; + joint.setCommand(wheel_power_limitor_.torque[i] * wheel_power_limitor_.K_angle[i]); + } + } +} + +void SwerveController::updatePowerStatus() +{ + double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + + pivot_power_limitor_.max_power = pivot_power_limitor_.ratio * power_limit; + + double epivot_power{}, cpivot_power{}; + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + + double cmd_torque = pivot_power_limitor_.torque[i] = module.ctrl_pivot_->joint_.getCommand(); + double real_vel = pivot_power_limitor_.omiga[i] = module.ctrl_pivot_->joint_.getVelocity(); + motor_lp_filters_[0][i]->input(module.ctrl_pivot_->joint_.getEffort()); + double real_torque = motor_lp_filters_[0][i]->output(); + + epivot_power += real_torque * real_vel + pivot_power_limitor_.effort_coeff * square(real_torque) + + pivot_power_limitor_.vel_coeff * abs(real_vel); + cpivot_power += cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * abs(real_vel); + + pivot_power_limitor_.power_in[i] = cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * abs(real_vel); + } + + pivot_power_limitor_.cmd_power = cpivot_power + pivot_power_limitor_.power_offset; + pivot_power_limitor_.estimated_power = epivot_power + pivot_power_limitor_.power_offset; + + wheel_power_limitor_.max_power = power_limit - std::abs(pivot_power_limitor_.cmd_power); + wheel_power_limitor_.err_sum = 0; + + double ewheel_power{}, cwheel_power{}; + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + + double cmd_torque = wheel_power_limitor_.torque[i] = module.ctrl_wheel_->joint_.getCommand(); + double cmd_vel{}; + module.ctrl_wheel_->getCommand(cmd_vel); + double real_vel = wheel_power_limitor_.omiga[i] = module.ctrl_wheel_->joint_.getVelocity(); + motor_lp_filters_[1][i]->input(module.ctrl_wheel_->joint_.getEffort()); + double real_torque = motor_lp_filters_[1][i]->output(); + + wheel_power_limitor_.err[i] = cmd_vel - real_vel; + wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); + + ewheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + + wheel_power_limitor_.power_in[i] = cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + } + wheel_power_limitor_.cmd_power = cwheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.estimated_power = ewheel_power + wheel_power_limitor_.power_offset; + + double estimated_total_power = pivot_power_limitor_.estimated_power + wheel_power_limitor_.estimated_power; + double cmd_total_power = pivot_power_limitor_.cmd_power + wheel_power_limitor_.cmd_power; + + if (capacity_update_flag_ && use_rls_) + { + double all_in = limit(estimated_total_power, -power_limit, power_limit); + + // Update Rls: compute regression vector x and pass actual measured power + Eigen::Matrix x; + x(0) = square(pivot_power_limitor_.torque[0]) + square(pivot_power_limitor_.torque[1]) + + square(pivot_power_limitor_.torque[2]) + square(pivot_power_limitor_.torque[3]); + x(1) = abs(pivot_power_limitor_.omiga[0]) + abs(pivot_power_limitor_.omiga[1]) + + abs(pivot_power_limitor_.omiga[2]) + abs(pivot_power_limitor_.omiga[3]); + x(2) = square(wheel_power_limitor_.torque[0]) + square(wheel_power_limitor_.torque[1]) + + square(wheel_power_limitor_.torque[2]) + square(wheel_power_limitor_.torque[3]); + x(3) = abs(wheel_power_limitor_.omiga[0]) + abs(wheel_power_limitor_.omiga[1]) + + abs(wheel_power_limitor_.omiga[2]) + abs(wheel_power_limitor_.omiga[3]); + rls_->setU(all_in); + rls_->setX(x); + rls_->setY(chassis_power_); + rls_->update(); // Internally computes predicted output as x^T * w + auto w = rls_->getW(); + pivot_power_limitor_.effort_coeff = std::max(w(0), 1e-3); + pivot_power_limitor_.vel_coeff = std::max(w(1), 1e-3); + wheel_power_limitor_.effort_coeff = std::max(w(2), 1e-3); + wheel_power_limitor_.vel_coeff = std::max(w(3), 1e-3); + capacity_update_flag_ = false; + } + + // Publish power status. + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(epower_pub_, estimated_total_power); + publishPower(cpower_pub_, cmd_total_power); +} + PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/test/omni.yaml b/rm_chassis_controllers/test/omni.yaml index 564ecd64..ce352f09 100644 --- a/rm_chassis_controllers/test/omni.yaml +++ b/rm_chassis_controllers/test/omni.yaml @@ -10,6 +10,7 @@ controllers: power_offset: 0 twist_angular: 0.5233 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] diff --git a/rm_chassis_controllers/test/swerve.yaml b/rm_chassis_controllers/test/swerve.yaml index 2c1bc975..9dc0a189 100644 --- a/rm_chassis_controllers/test/swerve.yaml +++ b/rm_chassis_controllers/test/swerve.yaml @@ -9,6 +9,7 @@ controllers: vel_coeff: 0.0048 power_offset: -3 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] pid_follow: { p: 5, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } # SwerveController diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98e8736b..a92c2890 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -65,7 +65,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 3cf38627..0abda770 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -10,22 +10,26 @@ gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) +gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) +gen.add("outpost_delay", double_t, 0, "Delay of bullet firing of outpost", 0.0, 0, 0.5) gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5) gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) gen.add("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 20) -gen.add("switch_duration_scale",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_rate",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_offset",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) -gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4) +gen.add("outpost_switch_angle_offset", double_t, 0,"Switch angle offset", 0.0, -20.0, 20) +gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99) +gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) +gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -400.0, 400.0) +gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0) +gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) +gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 5f614c33..6c6a3107 100644 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -13,5 +13,6 @@ gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_d_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) +gen.add("moment_of_inertia_", double_t, 0, "Moment of inertia of gimbal", 0.0, 0, 999.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 163f8475..d0b3cb32 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,15 +57,15 @@ namespace rm_gimbal_controllers { struct Config { - double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale, - switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay, - track_move_target_delay; - int min_fit_switch_count; - int max_selected_armor_; + double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, + resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, outpost_delay, center_delay, + max_switch_angle, switch_angle_offset, outpost_switch_angle_offset, min_shoot_beforehand_vel, + track_rotate_target_delay, track_move_target_delay, yaw_max_acc, track_rotate_outpost_delay; + int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; double end_pos_offset; + double traject_k_effort, traject_k_vel_; }; struct TrajectoryFunctionCoefficients { @@ -82,22 +82,27 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel); + double r1, double r2, double dz, double armors_num, double start_vel, double track_id); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); - double getResistanceCoefficient(double bullet_speed) const; + double r2, double dz, double armors_num, double yaw_real, double pitch_real, + double bullet_speed); + double getResistanceCoefficient(double target_distance) const; double getYaw() const { - return output_yaw_; + return output_yaw_[0]; } double getPitch() const { - return -output_pitch_; + return -output_pitch_[0]; } double getTrajectYaw() const { return traject_output_yaw_; } + double getTrajectVel() const + { + return traject_vel_; + } bool getUsingtraject() const { return using_traject_; @@ -110,26 +115,32 @@ class BulletSolver { return traject_effort_ff_; } - bool getTrackTarget() + bool getTrackTarget() const { return track_target_; } - double getGimbalSwitchDuration(double v_yaw); + void CleanTrackCount() + { + track_count_ = 0; + } + double getFlyTime() + { + return fly_time_[0]; + } void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num); - void judgeShootBeforehand(const ros::Time& time, double v_yaw); + double r1, double r2, double dz, double armors_num); + uint8_t judgeShootBeforehand(double v_yaw, int id); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); - double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_); + double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw); void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> shoot_beforehand_cmd_pub_; std::shared_ptr> fly_time_pub_; std::shared_ptr> bullet_solver_pub; ros::Subscriber identified_target_change_sub_; @@ -138,46 +149,56 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; - double max_track_target_vel_; - double output_yaw_{}, output_pitch_{}, traject_output_yaw_{}; + double yaw_[150], pos_x[150], pos_y[150]; + double max_track_target_vel_{}; + double output_yaw_[150], output_pitch_[150], traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; - double fly_time_; - double switch_hysteresis_; + double fly_time_[150]; + double switch_hysteresis_{}; double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; - double yaw_subtract_; - double switch_armor_angle; + double yaw_subtract_{}; + double switch_armor_angle{}; double filtered_v_yaw_{}; - double switchtime; - double traject_effort_ff_; - double traject_switch_time_; - double switche_time_yaw_; - double traject_max_acc_; - int shoot_num_ = 0; - int shoot_beforehand_cmd_{}; - int count_; - int traject_count_; - int ban_shoot_count_ = 0; - int selected_armor_ = 0; + double switchtime{}; + double traject_effort_ff_{}; + double traject_switch_time_{}; + double switche_time_yaw_{}; + double traject_max_acc_{}; + double last_output_yaw_{}; + double traject_vel_{}; + double r_traject_{}; + double intital_yaw_{}; - bool track_target_ = true; + int shoot_num_{}; + int shoot_beforehand_cmd_{}; + int count_[150]{}; + int next_count_[150]{}; + int ban_shoot_count_{}; + int selected_armor_[150] = {}; + int last_selected_armor_ = {}; + bool track_target_ = false; + int track_count_{}; bool identified_target_change_ = true; - bool is_in_delay_before_switch_{}; bool dynamic_reconfig_initialized_{}; - bool change_armor = false; - bool using_traject_; - bool last_shoot_state_; + bool using_traject_{}; + bool last_shoot_state_{}; + bool is_aheading_two_[150]{}; + bool start_traject_{}; + double filtered_vel_des{}; geometry_msgs::Point after_traject_output_yaw_{}; - geometry_msgs::Point target_pos_{}; + geometry_msgs::Point target_pos_[150]{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; - ros::Time start_using_traject_time; - ros::Time ban_shoot_time_; + ros::Time start_using_traject_time{}; + ros::Time ban_shoot_time_{}; + ros::Time last_output_time_{}; + ros::Time ban_shoot_start_time_{}; - TrajectoryFunctionCoefficients trajectory_function_coefficients; - TrajectoryLimitParams stauts_limit_; + TrajectoryFunctionCoefficients trajectory_function_coefficients{}; + TrajectoryLimitParams stauts_limit_{}; - mutable std::mutex heat_mutex_; + mutable std::mutex heat_mutex_{}; }; } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h new file mode 100644 index 00000000..85dccb45 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * BSD 3-Clause License + ******************************************************************************/ + +#pragma once + +#include + +namespace rm_gimbal_controllers +{ +class DualYawController : public Controller +{ +public: + DualYawController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +protected: + bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf, + int axis) const override; + bool shouldApplyJointLimit(int axis) const override; + std::string getBaseFrameID(const std::unordered_map& joint_urdfs) override; + void updateYawJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel, + const double pos_real[3], const double pos_des[3], const double pos_des_temp[3], + const double vel_des[3], const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]) override; + +private: + bool initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh); + bool getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point); + void publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error); + + urdf::JointConstSharedPtr base_yaw_joint_; + std::unique_ptr base_yaw_ctrl_; + std::unique_ptr base_yaw_pid_pos_; + std::unique_ptr> base_yaw_pos_state_pub_; +}; +} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 50c3eff2..c736107c 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -44,12 +44,12 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -57,22 +57,22 @@ #include #include #include -#include -#include -#include +#include namespace rm_gimbal_controllers { struct GimbalConfig { double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; - double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel { public: - explicit ChassisVel(const ros::NodeHandle& nh) + ChassisVel(const ros::NodeHandle& nh) { double num_data; nh.param("num_data", num_data, 20.0); @@ -141,26 +141,44 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); + void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); + bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - void updateChassisVel(); - void updateBallisticSolution(const ros::Time& time); double gravityFeedForward(const ros::Time& time); + void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); + void publishShootBeforehand(const ros::Time& time, uint8_t cmd); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); - void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg); + void mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); - std::string getGimbalFrameID(std::unordered_map joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; @@ -169,34 +187,41 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs_; std::unordered_map pos_des_in_limit_; bool has_imu_ = true; - double ballistic_yaw_{}, ballistic_pitch_{}; std::shared_ptr bullet_solver_; - std::shared_ptr ballistic_solver_; + double gimbal_real_z_vel_{}; + enum class TrackSolverType + { + BULLET_SOLVER, + EXTERNAL_MPC_AIM + }; + TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER }; + double external_aim_timeout_{ 0.1 }; + bool external_aim_active_{ false }; // ROS Interface ros::Time last_publish_time_{}; - ros::Time last_ballistic_publish_time_{}; + ros::Time last_track_time_{}; std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; - std::shared_ptr> ballistic_solution_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; - ros::Subscriber ballistic_solver_request_sub_; + ros::Subscriber mpc_gimbal_aim_sub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - realtime_tools::RealtimeBuffer ballistic_track_rt_buffer_; + realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; + rm_msgs::MPCGimbalAimCmd gimbal_aim_; std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_; double publish_rate_{}; bool state_changed_{}; int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, base2gimbal_, - odom2gimbal_traject_des_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; @@ -221,9 +246,11 @@ class Controller : public controller_interface::MultiInterfaceController + + + The DualYawController controls base_yaw, yaw and pitch from one GimbalCmd source. + + + diff --git a/rm_gimbal_controllers/src/ballistic_solver.cpp b/rm_gimbal_controllers/src/ballistic_solver.cpp index 97131f3b..f9c8284d 100644 --- a/rm_gimbal_controllers/src/ballistic_solver.cpp +++ b/rm_gimbal_controllers/src/ballistic_solver.cpp @@ -64,9 +64,9 @@ void BallisticSolver::solver(const geometry_msgs::TransformStamped& base2gimbal, double error_after_step = error_function(current_pitch + config.newton_pitch_epsilon); double error_derivative = (error_after_step - error) / config.newton_pitch_epsilon; double pitch_adjustment = error / error_derivative; - pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? -config.max_newton_step : - (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : - pitch_adjustment; + pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? + -config.max_newton_step : + (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : pitch_adjustment; double update_pitch = current_pitch - pitch_adjustment; double update_error = error_function(update_pitch); if (std::abs(update_error) < config.newton_convergence_tol) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 06f330f3..a604342d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -51,22 +52,25 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .resistance_coff_qd_16 = getParam(controller_nh, "resistance_coff_qd_16", 0.), .resistance_coff_qd_18 = getParam(controller_nh, "resistance_coff_qd_18", 0.), .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), + .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 0.001), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), + .outpost_delay = getParam(controller_nh, "outpost_delay", 0.), .center_delay = getParam(controller_nh, "center_delay", 0.0), - .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.0), - .switch_duration_scale = getParam(controller_nh, "switch_duration_scale", 0.), - .switch_duration_rate = getParam(controller_nh, "switch_duration_rate", 0.), - .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.1), - .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0), + .outpost_switch_angle_offset = getParam(controller_nh, "outpost_switch_angle_offset", 0.0), + .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 2.5), .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), + .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), + .track_rotate_outpost_delay = getParam(controller_nh, "track_rotate_outpost_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), - .max_selected_armor_ = getParam(controller_nh, "max_selecteed_armor", 3), + .traject_start_fit_ = getParam(controller_nh, "traject_start_fit_", 45), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), + .traject_k_effort = getParam(controller_nh, "traject_k_effort", 0.0), + .traject_k_vel_ = getParam(controller_nh, "traject_k_vel", 0.65), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -96,8 +100,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - shoot_beforehand_cmd_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); bullet_solver_pub.reset( new realtime_tools::RealtimePublisher(controller_nh, "bullet_solver_data", 10)); @@ -107,352 +109,385 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) &BulletSolver::heatCB, this); } -double BulletSolver::getResistanceCoefficient(double bullet_speed) const +double BulletSolver::getResistanceCoefficient(double target_distance) const { - // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - if (bullet_speed < 12.5) - resistance_coff = config_.resistance_coff_qd_10; - else if (bullet_speed < 15.5) - resistance_coff = config_.resistance_coff_qd_15; - else if (bullet_speed < 17) - resistance_coff = config_.resistance_coff_qd_16; - else if (bullet_speed < 24) - resistance_coff = config_.resistance_coff_qd_18; - else - resistance_coff = config_.resistance_coff_qd_30; + // hero + resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) + double v_yaw, double r1, double r2, double dz, double armors_num, double start_vel, + double track_id) { + while (yaw > M_PI / 2) + yaw -= M_PI; + while (yaw < -M_PI / 2) + yaw += M_PI; + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; - resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; if (abs(yaw - last_yaw_) > 1.) filtered_yaw_ = yaw; else if (last_yaw_ != yaw) - { filtered_yaw_ = filtered_yaw_ + (yaw - filtered_yaw_) * (0.001 / (0.01 + 0.001)); - } last_yaw_ = yaw; filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; - double temp_z = pos.z; - double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - double output_yaw_central = std::atan2(pos.y, pos.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); - double rough_fly_time = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double r = r1; - double z = pos.z; - if (track_target_) - yaw += filtered_v_yaw_ * config_.track_rotate_target_delay; - pos.x += vel.x * config_.track_move_target_delay; - pos.y += vel.y * config_.track_move_target_delay; - if (track_target_) + if (track_count_ <= 300) { - if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) - track_target_ = false; + track_target_ = false; + track_count_++; } - else + if (track_count_ > 300) { - if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) + if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) + track_target_ = false; + else if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; - } - switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; - traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_; - // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; - // yaw_subtract_ = filtered_yaw_ - output_yaw_central; - yaw_subtract_ = filtered_yaw_ - output_yaw_central; - while (yaw_subtract_ > M_PI) - yaw_subtract_ -= 2 * M_PI; - while (yaw_subtract_ < -M_PI) - yaw_subtract_ += 2 * M_PI; - double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; - if (((after_fly_yaw_subtract_ < switch_armor_angle - 0.3 && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ > switch_armor_angle + 0.3 && filtered_v_yaw_ < -1.)) && - track_target_) - { - selected_armor_ = 0; - } - else if (!track_target_) - { - selected_armor_ = 0; + if (sqrt(pow(pos.x, 2) + pow(pos.y, 2)) < 1.2) + track_target_ = false; } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) + if (identified_target_change_) { - count_++; - if (change_armor) + for (int i = 0; i < 150; i++) { - count_ = 0; - change_armor = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; + count_[i] = 0; + next_count_[i] = 0; } - if (count_ >= config_.min_fit_switch_count) + identified_target_change_ = false; + } + for (int i = 0; i < 150; i++) + { + double temp_z = pos.z; + if (track_target_) { - selected_armor_ = v_yaw > 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = pos.z + dz; - if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) - { - selected_armor_ = -2; - } - else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.) + if (track_id == 6) { - selected_armor_ = 2; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_outpost_delay + i * 0.001); } - if (count_ == config_.min_fit_switch_count) + else { - switch_armor_time_ = ros::Time::now(); - traject_count_ = 0; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); } } - } - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 2. && - count_ > 2) - { - traject_count_++; - if (traject_count_ == 2) + else + yaw_[i] = yaw; + pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.001); + pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.001); + double target_rho = std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2)); + resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; + double output_yaw_central = std::atan2(pos_y[i], pos_x[i]); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2))); + double rough_fly_time = + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + if (bullet_speed == 0.0) { - using_traject_ = true; - start_using_traject_time = ros::Time::now(); + ROS_ERROR("solve bullet speed is zero"); + return false; } - } - double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.57; - double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_central; - while (aim_yaw_subtract > M_PI) - aim_yaw_subtract -= 2 * M_PI; - while (aim_yaw_subtract < -M_PI) - aim_yaw_subtract += 2 * M_PI; - is_in_delay_before_switch_ = - ((((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) > switch_armor_angle) && filtered_v_yaw_ > 1.) || - (((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) < -switch_armor_angle) && filtered_v_yaw_ < -1.)) && - track_target_; - int count{}; - double error = 999; - if (track_target_) - { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.57); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.57); - } - else - { - target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); - target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.5) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5)) + double r = r1; + double z = pos.z; + + switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; + if (track_id == 6) { - selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; + switch_armor_angle = acos(r1 / target_rho) - config_.outpost_switch_angle_offset; } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.5))) + yaw_subtract_ = yaw_[i] - output_yaw_central; + // while (yaw_subtract_ > M_PI) + // yaw_subtract_ -= 2 * M_PI; + // while (yaw_subtract_ < -M_PI) + // yaw_subtract_ += 2 * M_PI; + double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; + if (abs(v_yaw) < 2.) + selected_armor_[i] = 0; + else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.4 && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ > -switch_armor_angle + 0.4 && filtered_v_yaw_ < -2.)) + selected_armor_[i] = 0; + + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) { - selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; + count_[i]++; + if (count_[i] >= config_.min_fit_switch_count) + { + selected_armor_[i] = v_yaw > 0. ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = pos.z + dz; + if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) + { + selected_armor_[i] = -2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) + { + selected_armor_[i] = 2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + if (count_[i] == config_.min_fit_switch_count) + { + if (is_aheading_two_[i]) + { + is_aheading_two_[i] = false; + } + else + { + switch_armor_time_ = ros::Time::now(); + } + } + } } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.5))) + int count{}; + double error = 999; + if (track_target_) { - selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; + target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); } - if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || - (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_)) + else { - selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); + target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); + selected_armor_[i] = 0; + if ((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 1 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 1 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 2 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 2 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; + } + + if (selected_armor_[i] % 2 == 0) + { + r = r1; + z = pos.z; + } + else + { + r = r2; + z = pos.z + dz; + } } - if (selected_armor_ % 2 == 0) + target_pos_[i].z = z; + while (error >= 0.001) { - r = r1; - z = pos.z; + output_yaw_[i] = std::atan2(target_pos_[i].y, target_pos_[i].x); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2))); + target_rho = std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2)); + fly_time_[i] = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + double real_z = (bullet_speed_ * std::sin(output_pitch_[i]) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_[i])) / resistance_coff_ - + config_.g * fly_time_[i] / resistance_coff_; + if (track_target_) + { + target_pos_[i].x = pos_x[i] + vel.x * fly_time_[i] - + r * cos(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] + vel.y * fly_time_[i] - + r * sin(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos_x[i] + vel.x * fly_time_[i]; + target_pos_after_fly_time[1] = pos_y[i] + vel.y * fly_time_[i]; + target_pos_[i].x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_[i].y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } + target_pos_[i].z = z + vel.z * fly_time_[i]; + + double target_yaw = std::atan2(target_pos_[i].y, target_pos_[i].x); + double error_theta = target_yaw - output_yaw_[i]; + double error_z = target_pos_[i].z - real_z; + temp_z += error_z; + error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); + count++; + if (count >= 20 || std::isnan(error)) + return false; } } - target_pos_.z = z; - double r_trajcet_; - if (selected_armor_ == 1 || selected_armor_ == -1) + if (fly_time_pub_->trylock()) { - r_trajcet_ = r1; + fly_time_pub_->msg_.data = fly_time_[0]; + fly_time_pub_->unlockAndPublish(); + } + if (selected_armor_[0] == 1 || selected_armor_[0] == -1 || selected_armor_[0] == 3 || selected_armor_[0] == -3) + { + r_traject_ = r1; } else { - r_trajcet_ = r2; + r_traject_ = r2; } - - if (!using_traject_) + for (int i = 1; i <= config_.traject_start_fit_; i++) { - switchtime = 0.08; - - if (filtered_v_yaw_ > 0) - { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - } - else + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 4.5) { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + start_traject_ = true; + using_traject_ = true; + start_using_traject_time = ros::Time::now(); + break; } - stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = start_vel; - stauts_limit_.end_pos = - std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; - stauts_limit_.end_vel = stauts_limit_.start_vel; + } - if (switchtime > 0.2) + if (start_traject_ && using_traject_) + { + traject_max_acc_ = 400.; + switchtime = 0.05; + while (abs(traject_max_acc_) > config_.yaw_max_acc) { - using_traject_ = false; - } - trajectory_function_coefficients.a0 = stauts_limit_.start_pos; - trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - - Eigen::Matrix2d A; - A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); - Eigen::Vector2d B; - B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), - stauts_limit_.end_vel - stauts_limit_.start_vel; + switchtime += 0.005; + if (filtered_v_yaw_ > 0) + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + } + else + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + } + stauts_limit_.start_pos = output_yaw_[0]; + stauts_limit_.start_vel = start_vel; + stauts_limit_.end_pos = + std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; + stauts_limit_.end_pos = + stauts_limit_.start_pos + angles::shortest_angular_distance(stauts_limit_.start_pos, stauts_limit_.end_pos); + stauts_limit_.end_vel = stauts_limit_.start_vel; - Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); + if (switchtime > 0.15) + { + break; + } + trajectory_function_coefficients.a0 = stauts_limit_.start_pos; + trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - trajectory_function_coefficients.a2 = X(0); - trajectory_function_coefficients.a3 = X(1); - traject_max_acc_ = 2 * trajectory_function_coefficients.a2; - } + Eigen::Matrix2d A; + A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); + Eigen::Vector2d B; + B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), + stauts_limit_.end_vel - stauts_limit_.start_vel; - while (error >= 0.001) - { - output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2))); - target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); - fly_time_ = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; + Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); - if (track_target_) - { - target_pos_.x = - pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = - pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); + trajectory_function_coefficients.a2 = X(0); + trajectory_function_coefficients.a3 = X(1); + traject_max_acc_ = abs(2 * trajectory_function_coefficients.a2); } - else - { - double target_pos_after_fly_time[2]; - target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_; - target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_; - target_pos_.x = - target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - target_pos_.y = - target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - } - target_pos_.z = z + vel.z * fly_time_; - - double target_yaw = std::atan2(target_pos_.y, target_pos_.x); - double error_theta = target_yaw - output_yaw_; - double error_z = target_pos_.z - real_z; - temp_z += error_z; - error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); - count++; - - if (count >= 20 || std::isnan(error)) - return false; - } - if (fly_time_pub_->trylock()) - { - fly_time_pub_->msg_.data = fly_time_; - fly_time_pub_->unlockAndPublish(); + start_traject_ = false; } if (using_traject_) { ros::Time temp = ros::Time::now(); - traject_output_yaw_ = planningPoint(temp, start_using_traject_time); + traject_output_yaw_ = planningPoint(temp, start_using_traject_time, v_yaw); + while (traject_output_yaw_ > M_PI) + traject_output_yaw_ -= 2 * M_PI; + while (traject_output_yaw_ < -M_PI) + traject_output_yaw_ += 2 * M_PI; if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; } } - else + if (!using_traject_ || !track_target_) { - traject_output_yaw_ = output_yaw_; + traject_output_yaw_ = output_yaw_[0]; } + double vel_des = (traject_output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + filtered_vel_des = filtered_vel_des + (vel_des - filtered_vel_des) * (0.001 / (0.01 + 0.001)); + last_output_time_ = ros::Time::now(); + last_output_yaw_ = traject_output_yaw_; if (bullet_solver_pub->trylock()) { - bullet_solver_pub->msg_.selected_armor_ = selected_armor_; - bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_; - bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; - bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; - bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = traject_switch_time_; - bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; - bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; - bullet_solver_pub->msg_.test_number = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - if (v_yaw > 1.0) + for (int i = 0; i < 150; ++i) { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 3 * 2 * M_PI / armors_num); - } - else if (v_yaw < -1.0) - { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 3 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.selected_armor_[i] = static_cast(selected_armor_[i]); + bullet_solver_pub->msg_.count_number_[i] = static_cast(count_[i]); } + bullet_solver_pub->msg_.start_vel = stauts_limit_.start_vel; + bullet_solver_pub->msg_.switchtime = switchtime; + bullet_solver_pub->msg_.traject_output_yaw_ = traject_output_yaw_; + bullet_solver_pub->msg_.traject_acc_ = traject_effort_ff_; + bullet_solver_pub->msg_.traject_vel_ = filtered_vel_des; + bullet_solver_pub->msg_.using_traject_ = using_traject_; + bullet_solver_pub->msg_.test_number[0] = yaw_[0]; bullet_solver_pub->unlockAndPublish(); } return true; } + void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num) + double v_yaw, double r1, double r2, double dz, double armors_num) { + if (armors_num == 3) + { + armors_num = 3.6; + } double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_ != 0 && armors_num == 3) + if (armors_num == 4 && selected_armor_[0] % 2 != 0) { r = r2; z = pos.z + dz; } - pos.x += vel.x * (config_.track_move_target_delay + fly_time_); - pos.y += vel.y * (config_.track_move_target_delay + fly_time_); + pos.x += vel.x * (config_.track_move_target_delay + fly_time_[0]); + pos.y += vel.y * (config_.track_move_target_delay + fly_time_[0]); if (track_target_) { - armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); - armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_pos.z = z; armor_vel.x = vel.x + v_yaw * r * - sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.y = vel.y - v_yaw * r * - cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.z = vel.z; } else @@ -470,18 +505,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi double roll{}, pitch{}, yaw{}; quatToRPY(odom2pitch.transform.rotation, roll, pitch, yaw); geometry_msgs::Point point_desire{}, point_real{}; - double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + double target_rho = std::sqrt(std::pow(target_pos_[0].x, 2) + std::pow(target_pos_[0].y, 2)); int point_num = int(target_rho * 20); for (int i = 0; i <= point_num; i++) { double rt_bullet_rho = target_rho * i / point_num; - double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / + double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[0])))) / resistance_coff_; - double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * + double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_[0]) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + odom2pitch.transform.translation.x; - point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + odom2pitch.transform.translation.y; + point_desire.x = rt_bullet_rho * std::cos(output_yaw_[0]) + odom2pitch.transform.translation.x; + point_desire.y = rt_bullet_rho * std::sin(output_yaw_[0]) + odom2pitch.transform.translation.y; point_desire.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_desire_.points.push_back(point_desire); } @@ -513,14 +548,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, - double bullet_speed) + double r1, double r2, double dz, double armors_num, double yaw_real, + double pitch_real, double bullet_speed) { + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); double delay; delay = track_target_ ? 0. : config_.center_delay; double r, z; - if (selected_armor_ % 2 == 0) + if (selected_armor_[0] % 2 == 0) { r = r1; z = pos.z; @@ -534,28 +573,28 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec if (track_target_) { double bullet_rho = - bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_; double bullet_x = bullet_rho * std::cos(yaw_real); double bullet_y = bullet_rho * std::sin(yaw_real); double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; - error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) + - std::pow(target_pos_.z - bullet_z, 2)); + (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_ - + config_.g * fly_time_[0] / resistance_coff_; + error = std::sqrt(std::pow(target_pos_[0].x - bullet_x, 2) + std::pow(target_pos_[0].y - bullet_y, 2) + + std::pow(target_pos_[0].z - bullet_z, 2)); } else { geometry_msgs::Point target_pos_after_fly_time_and_delay{}; target_pos_after_fly_time_and_delay.x = - pos.x + vel.x * (fly_time_ + delay) - - r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + pos.x + vel.x * (fly_time_[0] + delay) - + r * cos(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); target_pos_after_fly_time_and_delay.y = - pos.y + vel.y * (fly_time_ + delay) - - r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); - error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + - std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + - std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); + pos.y + vel.y * (fly_time_[0] + delay) - + r * sin(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_[0] + delay); + error = std::sqrt(std::pow(target_pos_[0].x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_[0].y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_[0].z - target_pos_after_fly_time_and_delay.z, 2)); } return error; } @@ -566,47 +605,53 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) identified_target_change_ = true; else if (!msg->data && identified_target_change_) { - change_armor = true; identified_target_change_ = false; } } -double BulletSolver::getGimbalSwitchDuration(double v_yaw) -{ - gimbal_switch_duration_ = - config_.switch_duration_scale * std::exp(config_.switch_duration_rate * (v_yaw * 60 / (2 * M_PI))) + - config_.switch_duration_offset; - return gimbal_switch_duration_; -} - -void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) +uint8_t BulletSolver::judgeShootBeforehand(double v_yaw, int id) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { - if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime)) + int ban_shoot_delay_number = static_cast(55 + config_.delay * 1000); + if (id == 6) + { + ban_shoot_delay_number = static_cast(55 + config_.outpost_delay * 1000); + } + ban_shoot_delay_number = ban_shoot_delay_number > 149 ? 149 : ban_shoot_delay_number; + if (selected_armor_[ban_shoot_delay_number] != selected_armor_[0]) + { + ban_shoot_count_++; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + if (ban_shoot_count_ == 1) + { + ban_shoot_start_time_ = ros::Time::now(); + } + } + else if (using_traject_ && ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (ros::Time::now().toSec() > - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime && - ros::Time::now().toSec() < - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime + config_.delay) + } + else if (ros::Time::now().toSec() > ban_shoot_start_time_.toSec() + switchtime && + ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime + config_.delay) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + ban_shoot_count_ = 0; + } else + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + ban_shoot_count_ = 0; + } } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; - if (shoot_beforehand_cmd_pub_->trylock()) - { - shoot_beforehand_cmd_pub_->msg_.stamp = time; - shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; - shoot_beforehand_cmd_pub_->unlockAndPublish(); - } + return static_cast(shoot_beforehand_cmd_); } -double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_) +double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw) { double a0 = trajectory_function_coefficients.a0; double a1 = trajectory_function_coefficients.a1; @@ -614,8 +659,8 @@ double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_ double a3 = trajectory_function_coefficients.a3; double n_time_ = (time - start_trajectory_time_).toSec(); double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3); - // traject_effort_ff_ = 1.4 - 1.4 * n_time_/switchtime; - traject_effort_ff_ = 0.0; + traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; + traject_vel_ = (a1 + 2 * a2 * n_time_ + 3 * a3 * pow(n_time_, 2)) * config_.traject_k_vel_; return plansetpoint; } @@ -646,20 +691,22 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30; config.g = init_config.g; config.delay = init_config.delay; + config.outpost_delay = init_config.outpost_delay; config.center_delay = init_config.center_delay; - config.max_switch_angle = init_config.max_switch_angle; config.switch_angle_offset = init_config.switch_angle_offset; - config.switch_duration_scale = init_config.switch_duration_scale; - config.switch_duration_rate = init_config.switch_duration_rate; - config.switch_duration_offset = init_config.switch_duration_offset; + config.outpost_switch_angle_offset = init_config.outpost_switch_angle_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; + config.yaw_max_acc = init_config.yaw_max_acc; + config.track_rotate_outpost_delay = init_config.track_rotate_outpost_delay; config.min_fit_switch_count = init_config.min_fit_switch_count; - config.max_selected_armor_ = init_config.max_selected_armor_; + config.traject_start_fit_ = init_config.traject_start_fit_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; + config.traject_k_effort = init_config.traject_k_effort; + config.traject_k_vel = init_config.traject_k_vel_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -667,22 +714,25 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_16 = config.resistance_coff_qd_16, .resistance_coff_qd_18 = config.resistance_coff_qd_18, .resistance_coff_qd_30 = config.resistance_coff_qd_30, + .resistance_coff_qd_800 = config.resistance_coff_qd_800, .g = config.g, .delay = config.delay, + .outpost_delay = config.outpost_delay, .center_delay = config.center_delay, - .max_switch_angle = config.max_switch_angle, .switch_angle_offset = config.switch_angle_offset, - .switch_duration_scale = config.switch_duration_scale, - .switch_duration_rate = config.switch_duration_rate, - .switch_duration_offset = config.switch_duration_offset, + .outpost_switch_angle_offset = config.outpost_switch_angle_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, + .yaw_max_acc = config.yaw_max_acc, + .track_rotate_outpost_delay = config.track_rotate_outpost_delay, .min_fit_switch_count = config.min_fit_switch_count, - .max_selected_armor_ = config.max_selected_armor_, + .traject_start_fit_ = config.traject_start_fit_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, - .end_pos_offset = config.end_pos_offset }; + .end_pos_offset = config.end_pos_offset, + .traject_k_effort = config.traject_k_effort, + .traject_k_vel_ = config.traject_k_vel }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/dual_yaw_controller.cpp b/rm_gimbal_controllers/src/dual_yaw_controller.cpp new file mode 100644 index 00000000..69ffa1b4 --- /dev/null +++ b/rm_gimbal_controllers/src/dual_yaw_controller.cpp @@ -0,0 +1,154 @@ +#include "rm_gimbal_controllers/dual_yaw_controller.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace rm_gimbal_controllers +{ +bool DualYawController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + if (!initBaseYaw(robot_hw, controller_nh)) + return false; + return Controller::init(robot_hw, root_nh, controller_nh); +} + +bool DualYawController::initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) +{ + ros::NodeHandle base_yaw_nh(controller_nh, "controllers/base_yaw"); + ros::NodeHandle base_yaw_pid_nh(controller_nh, "controllers/base_yaw/pid_pos"); + + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + base_yaw_joint_ = urdf.getJoint(getParam(base_yaw_nh, "joint", std::string("base_yaw_joint"))); + if (!base_yaw_joint_) + { + ROS_ERROR("Could not find base_yaw joint in urdf"); + return false; + } + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); + base_yaw_ctrl_ = std::make_unique(); + base_yaw_pid_pos_ = std::make_unique(); + base_yaw_pos_state_pub_ = + std::make_unique>(base_yaw_nh, "pos_state", 1); + return base_yaw_ctrl_->init(effort_joint_interface, base_yaw_nh) && base_yaw_pid_pos_->init(base_yaw_pid_nh); +} + +bool DualYawController::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& joint_urdf, int axis) const +{ + return !(axis == 2 && base_yaw_joint_ && joint_urdf->name == base_yaw_joint_->name); +} + +bool DualYawController::shouldApplyJointLimit(int axis) const +{ + return axis != 2; +} + +std::string DualYawController::getBaseFrameID(const std::unordered_map& joint_urdfs) +{ + if (base_yaw_joint_) + return base_yaw_joint_->parent_link_name.c_str(); + return Controller::getBaseFrameID(joint_urdfs); +} + +void DualYawController::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double /*pos_des_temp*/[3], + const double vel_des[3], const double angle_error[3], + const double traject_pos_des[3], const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) + { + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); + } + + if (!base_yaw_ctrl_ || !base_yaw_pid_pos_) + return; + + double base_yaw_pos_real = 0.; + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + double roll{}, pitch{}; + quatToRPY(odom2base_yaw.transform.rotation, roll, pitch, base_yaw_pos_real); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + + double armor_set_point{}; + double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2]; + double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point); + base_yaw_pid_pos_->computeCommand(base_yaw_error, period); + base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() - chassis_vel_->angular_->z()); + base_yaw_ctrl_->update(time, period); + publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error); +} + +bool DualYawController::getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point) +{ + if (state_ != TRACK || !base_yaw_joint_ || data_track_.header.stamp.isZero()) + return false; + + geometry_msgs::Point target_pos = data_track_.position; + if (!std::isfinite(target_pos.x) || !std::isfinite(target_pos.y) || !std::isfinite(target_pos.z)) + return false; + + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + geometry_msgs::Point base_yaw_pos_odom; + base_yaw_pos_odom.x = odom2base_yaw.transform.translation.x; + base_yaw_pos_odom.y = odom2base_yaw.transform.translation.y; + base_yaw_pos_odom.z = odom2base_yaw.transform.translation.z; + + armor_set_point = std::atan2(target_pos.y - base_yaw_pos_odom.y, target_pos.x - base_yaw_pos_odom.x); + if (!std::isfinite(armor_set_point)) + return false; + + return true; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } +} + +void DualYawController::publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error) +{ + if (base_yaw_pos_state_pub_ && loop_count_ % 10 == 0 && base_yaw_pos_state_pub_->trylock()) + { + base_yaw_pos_state_pub_->msg_.header.stamp = time; + base_yaw_pos_state_pub_->msg_.set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.traject_set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.set_point_dot = vel_des[2]; + base_yaw_pos_state_pub_->msg_.process_value = base_yaw_pos_real; + base_yaw_pos_state_pub_->msg_.error = base_yaw_error; + base_yaw_pos_state_pub_->msg_.command = base_yaw_pid_pos_->getCurrentCmd(); + base_yaw_pos_state_pub_->msg_.shoot_number = bullet_solver_->getShootnum(); + base_yaw_pos_state_pub_->unlockAndPublish(); + } +} + +} // namespace rm_gimbal_controllers + +PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::DualYawController, controller_interface::ControllerBase) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e7c67730..92269905 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -36,6 +36,7 @@ // #include "rm_gimbal_controllers/gimbal_base.h" +#include #include #include #include @@ -48,27 +49,6 @@ namespace rm_gimbal_controllers { bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); - chassis_vel_ = std::make_shared(chassis_vel_nh); - ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); - bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); - ballistic_solver_ = std::make_shared(nh_ballistic_solver); - - config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), - .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), - .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), - .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), - .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), - .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - XmlRpc::XmlRpcValue xml_rpc_value; const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) @@ -82,7 +62,51 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - auto* effort_joint_interface = robot_hw->get(); + ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); + chassis_vel_ = std::make_shared(chassis_vel_nh); + ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); + bullet_solver_ = std::make_shared(nh_bullet_solver); + std::string track_solver_type; + controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); + if (track_solver_type == "external_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_WARN("[Gimbal] track_solver_type=external_aim is deprecated, using external_mpc_aim instead"); + } + else if (track_solver_type == "external_mpc_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_INFO("[Gimbal] TRACK mode uses external MPC aim solver"); + } + else if (track_solver_type != "bullet_solver") + { + track_solver_type_ = TrackSolverType::BULLET_SOLVER; + ROS_WARN("[Gimbal] Unknown track_solver_type '%s', falling back to bullet_solver", track_solver_type.c_str()); + } + controller_nh.param("external_aim_timeout", external_aim_timeout_, 0.1); + std::string external_mpc_aim_topic; + controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); + + config_ = { + .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), + .moment_of_inertia_ = getParam(controller_nh, "controllers/moment_of_inertia", 0.), + }; + + config_rt_buffer_.initRT(config_); + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; + d_srv_->setCallback(cb); + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { // Get URDF info about joint @@ -103,12 +127,15 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro return false; } const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.emplace(axis, std::move(joint_urdf)); - ctrls_.emplace(axis, std::make_unique()); - pid_pos_.emplace(axis, std::make_unique()); - pos_des_in_limit_.emplace(axis, true); - pos_state_pub_.emplace( - axis, std::make_unique>(nh, "pos_state", 1)); + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + ctrls_.insert(std::make_pair(axis, std::make_unique())); + pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); + pos_state_pub_.insert(std::make_pair( + axis, std::make_unique>(nh, "pos_state", 1))); + if (!ctrls_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) return false; } @@ -119,8 +146,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); - auto* imu_sensor_interface = robot_hw->get(); + hardware_interface::ImuSensorInterface* imu_sensor_interface = + robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { @@ -137,18 +166,26 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.header.frame_id = "odom"; odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_); odom2base_.transform.rotation.w = 1.; + odom2gimbal_traject_des_.header.frame_id = "odom"; odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; + rm_msgs::GimbalCmd default_cmd; + rm_msgs::TrackData default_track; + rm_msgs::MPCGimbalAimCmd default_aim; + cmd_rt_buffer_.initRT(default_cmd); + track_rt_buffer_.initRT(default_track); + gimbal_aim_rt_buffer_.initRT(default_aim); + cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); - ballistic_solver_request_sub_ = controller_nh.subscribe("/ballistic_solver_request", 1, - &Controller::ballisticSolverRequestCB, this); + data_track_sub_ = controller_nh.subscribe("/sp_vision/track", 1, &Controller::trackCB, this); + mpc_gimbal_aim_sub_ = + controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - ballistic_solution_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "ballistic_solution", 100)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); return true; } @@ -164,19 +201,18 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); odom2base_ = robot_state_handle_.lookupTransform("odom", odom2base_.child_frame_id, time); - base2gimbal_ = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, odom2gimbal_.child_frame_id, time); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(5, "%s\n", ex.what()); return; } - updateBallisticSolution(time); updateChassisVel(); if (state_ != cmd_gimbal_.mode) { @@ -189,7 +225,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - track(time); + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) + externalAimTrack(time); + else + track(time, period); break; case DIRECT: direct(time); @@ -201,24 +240,40 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { - tf2::Quaternion odom2base, odom2gimbal_des, base2gimbal_des, base2gimbal_traject_des; + tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); - tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des); + tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des_q); double current_rpy[3], des_rpy[3], traject_rpy[3]; - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), current_rpy[0], current_rpy[1], current_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), des_rpy[0], des_rpy[1], des_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, traject_yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), current_rpy[0], current_rpy[1], current_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), des_rpy[0], des_rpy[1], des_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, traject_yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + for (const auto& it : joint_urdfs_) { - bool update = (it.first != 1 && it.first != 2) || (it.first == 1 && update_pitch) || (it.first == 2 && update_yaw); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } + base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); base2gimbal_traject_des.setRPY(traject_rpy[0], traject_rpy[1], traject_rpy[2]); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); @@ -229,6 +284,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, void Controller::rate(const ros::Time& time, const ros::Duration& period) { + bullet_solver_->CleanTrackCount(); if (state_changed_) { // on enter state_changed_ = false; @@ -254,8 +310,16 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { + external_aim_active_ = false; + if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) + { + state_ = RATE; + state_changed_ = true; + rate(time, period); + return; + } if (state_changed_) { // on enter state_changed_ = false; @@ -273,7 +337,7 @@ void Controller::track(const ros::Time& time) { if (!data_track_.header.frame_id.empty()) { - auto transform = + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); @@ -296,8 +360,8 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); + data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); + publishShootBeforehand(time, bullet_solver_->judgeShootBeforehand(data_track_.v_yaw, data_track_.id)); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -324,6 +388,58 @@ void Controller::track(const ros::Time& time) } } +bool Controller::externalAimIsFresh(const ros::Time& time) const +{ + if (!gimbal_aim_.valid || gimbal_aim_.header.stamp.isZero()) + return false; + const double age = (time - gimbal_aim_.header.stamp).toSec(); + return age >= -external_aim_timeout_ && age <= external_aim_timeout_; +} + +void Controller::externalAimTrack(const ros::Time& time) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Gimbal] Enter TRACK with external aim"); + } + + external_aim_active_ = externalAimIsFresh(time); + if (external_aim_active_) + { + setDes(time, gimbal_aim_.target_yaw, gimbal_aim_.pitch, gimbal_aim_.yaw); + publishShootBeforehand(time, gimbal_aim_.shoot_cmd == 1 ? rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT : + rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + else + { + odom2gimbal_des_.header.stamp = time; + robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); + publishShootBeforehand(time, rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (error_pub_->trylock()) + { + error_pub_->msg_.stamp = time; + error_pub_->msg_.error = external_aim_active_ && std::isfinite(gimbal_aim_.error) ? gimbal_aim_.error : 1.0; + error_pub_->unlockAndPublish(); + } + last_publish_time_ = time; + } +} + +void Controller::publishShootBeforehand(const ros::Time& time, uint8_t cmd) +{ + if (shoot_beforehand_cmd_pub_ && shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = cmd; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void Controller::direct(const ros::Time& time) { if (state_changed_) @@ -335,11 +451,9 @@ void Controller::direct(const ros::Time& time) try { if (!cmd_gimbal_.target_pos.header.frame_id.empty()) - { - auto transform = robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, - cmd_gimbal_.target_pos.header.stamp); - tf2::doTransform(aim_point_odom, aim_point_odom, transform); - } + tf2::doTransform(aim_point_odom, aim_point_odom, + robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, + cmd_gimbal_.target_pos.header.stamp)); } catch (tf2::TransformException& ex) { @@ -419,9 +533,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) gyro.z = imu_sensor_handle_.getAngularVelocity()[2]; try { - auto transform = - robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), time); - tf2::doTransform(gyro, angular_vel, transform); + tf2::doTransform(gyro, angular_vel, + robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), + time)); } catch (tf2::TransformException& ex) { @@ -438,6 +552,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } + gimbal_real_z_vel_ = angular_vel.z; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); @@ -453,79 +568,83 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - if (data_track_.id != 12) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) { - geometry_msgs::Point pos = data_track_.position; - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); - pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); - pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); - pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, - data_track_.dz, data_track_.armors_num); + if (external_aim_active_ && externalAimIsFresh(time)) + { + vel_des[2] = gimbal_aim_.yaw_rate; + vel_des[1] = gimbal_aim_.pitch_rate; + } + else + { + vel_des[2] = 0.; + vel_des[1] = 0.; + } } else { - target_pos = data_track_.position; - target_vel = data_track_.velocity; - } - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - tf2::Vector3 target_pos_tf, target_vel_tf; - try - { - if (joint_urdfs_.find(2) != joint_urdfs_.end()) + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + if (data_track_.id != 12) { - auto transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + geometry_msgs::Point pos = data_track_.position; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); + pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); + pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); } - if (joint_urdfs_.find(1) != joint_urdfs_.end()) + else { - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + target_pos = data_track_.position; + target_vel = data_track_.velocity; + } + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + tf2::Vector3 target_pos_tf, target_vel_tf; + try + { + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); } } + for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - pid_pos_.at(2)->computeCommand(bullet_solver_->getUsingtraject() ? traject_angle_error[2] : angle_error[2], period); - double cmd = pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z; - if (state_ == TRACK && bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) - cmd += bullet_solver_->getTrajectEffortff(); - ctrls_.at(2)->setCommand(cmd); - ctrls_.at(2)->update(time, period); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -561,9 +680,9 @@ double Controller::gravityFeedForward(const ros::Time& time) if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, - joint_urdfs_.at(1)->parent_link_name, time); - tf2::doTransform(gravity_compensation, gravity_compensation, transform); + tf2::doTransform(gravity_compensation, gravity_compensation, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, + joint_urdfs_.at(1)->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -591,42 +710,97 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -void Controller::updateBallisticSolution(const ros::Time& time) +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) { - if (publish_rate_ > 0.0 && last_ballistic_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) { - if (ballistic_solution_pub_->trylock()) + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; + + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) + { + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else { - std_msgs::Float32MultiArray data; - data.data.emplace_back(ballistic_yaw_); - data.data.emplace_back(ballistic_pitch_); - ballistic_solution_pub_->msg_.data = data.data; - ballistic_solution_pub_->unlockAndPublish(); + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); } - last_ballistic_publish_time_ = time; } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + } + + ctrls_.at(2)->update(time, period); } -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->child_link_name; + return joint_urdfs.at(1)->child_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->child_link_name; + return joint_urdfs.at(0)->child_link_name.c_str(); if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->child_link_name; - return {}; + return joint_urdfs.at(2)->child_link_name.c_str(); + return std::string(); } -std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->parent_link_name; + return joint_urdfs.at(2)->parent_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->parent_link_name; + return joint_urdfs.at(0)->parent_link_name.c_str(); if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->parent_link_name; - return {}; + return joint_urdfs.at(1)->parent_link_name.c_str(); + return std::string(); } double Controller::updateCompensation(double chassis_vel_angular_z) @@ -649,12 +823,9 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } -void Controller::ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg) +void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) { - ballistic_track_rt_buffer_.writeFromNonRT(*msg); - bool ballistic_solver_request = msg->data; - if (ballistic_solver_request) - ballistic_solver_->solver(base2gimbal_, ballistic_yaw_, ballistic_pitch_); + gimbal_aim_rt_buffer_.writeFromNonRT(*msg); } void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) @@ -665,12 +836,13 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; - config.accel_pitch_ = init_config.accel_pitch_; - config.accel_yaw_ = init_config.accel_yaw_; config.chassis_comp_a_ = init_config.chassis_comp_a_; config.chassis_comp_b_ = init_config.chassis_comp_b_; config.chassis_comp_c_ = init_config.chassis_comp_c_; config.chassis_comp_d_ = init_config.chassis_comp_d_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; + config.moment_of_inertia_ = init_config.moment_of_inertia_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, @@ -680,7 +852,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, - .chassis_comp_d_ = config.chassis_comp_d_ }; + .chassis_comp_d_ = config.chassis_comp_d_, + .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); }