From 7ef980154e2d596fa408f54f038aec049808029c Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 11 Mar 2023 20:52:06 +0800 Subject: [PATCH 01/27] Try to use cascade pid to control the gimbal. --- .../rm_gimbal_controllers/gimbal_base.h | 6 +- rm_gimbal_controllers/src/gimbal_base.cpp | 99 ++++++++++++------- 2 files changed, 69 insertions(+), 36 deletions(-) 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 003d9dca..4152fe42 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -38,6 +38,7 @@ #pragma once #include +#include #include #include #include @@ -51,6 +52,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -171,7 +173,9 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index ae9a4237..84fd5c20 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -68,12 +68,33 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); - ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); + ros::NodeHandle nh_yaw_pos = ros::NodeHandle(controller_nh, "yaw_pos"); + ros::NodeHandle nh_pitch_pos = ros::NodeHandle(controller_nh, "pitch_pos"); + ros::NodeHandle nh_yaw_vel = ros::NodeHandle(controller_nh, "yaw_vel"); + ros::NodeHandle nh_pitch_vel = ros::NodeHandle(controller_nh, "pitch_vel"); + hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); - if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) + + if (!pos_ctrl_yaw_.init(effort_joint_interface, nh_yaw_pos) || + !pos_ctrl_pitch_.init(effort_joint_interface, nh_pitch_pos) || + !vel_ctrl_yaw_.init(effort_joint_interface, nh_yaw_vel) || + !vel_ctrl_pitch_.init(effort_joint_interface, nh_pitch_vel)) return false; + + if (controller_nh.hasParam("pos_pid_yaw")) + if (!pos_pid_yaw_.init(ros::NodeHandle(controller_nh, "pos_pid_yaw"))) + return false; + if (controller_nh.hasParam("pos_pid_pitch")) + if (!pos_pid_pitch_.init(ros::NodeHandle(controller_nh, "pos_pid_pitch"))) + return false; + if (controller_nh.hasParam("vel_pid_yaw")) + if (!vel_pid_yaw_.init(ros::NodeHandle(controller_nh, "vel_pid_yaw"))) + return false; + if (controller_nh.hasParam("vel_pid_pitch")) + if (!vel_pid_pitch_.init(ros::NodeHandle(controller_nh, "vel_pid_pitch"))) + return false; + robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); if (!controller_nh.hasParam("imu_name")) has_imu_ = false; @@ -89,15 +110,15 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - gimbal_des_frame_id_ = ctrl_pitch_.joint_urdf_->child_link_name + "_des"; + gimbal_des_frame_id_ = pos_ctrl_pitch_.joint_urdf_->child_link_name + "_des"; odom2gimbal_des_.header.frame_id = "odom"; odom2gimbal_des_.child_frame_id = gimbal_des_frame_id_; odom2gimbal_des_.transform.rotation.w = 1.; odom2pitch_.header.frame_id = "odom"; - odom2pitch_.child_frame_id = ctrl_pitch_.joint_urdf_->child_link_name; + odom2pitch_.child_frame_id = pos_ctrl_pitch_.joint_urdf_->child_link_name; odom2pitch_.transform.rotation.w = 1.; odom2base_.header.frame_id = "odom"; - odom2base_.child_frame_id = ctrl_yaw_.joint_urdf_->parent_link_name; + odom2base_.child_frame_id = pos_ctrl_yaw_.joint_urdf_->parent_link_name; odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); @@ -120,8 +141,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) data_track_ = *track_rt_buffer_.readFromNonRT(); try { - odom2pitch_ = robot_state_handle_.lookupTransform("odom", ctrl_pitch_.joint_urdf_->child_link_name, time); - odom2base_ = robot_state_handle_.lookupTransform("odom", ctrl_yaw_.joint_urdf_->parent_link_name, time); + odom2pitch_ = robot_state_handle_.lookupTransform("odom", pos_ctrl_pitch_.joint_urdf_->child_link_name, time); + odom2base_ = robot_state_handle_.lookupTransform("odom", pos_ctrl_yaw_.joint_urdf_->parent_link_name, time); } catch (tf2::TransformException& ex) { @@ -160,13 +181,13 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(base2gimbal_des), roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw); double pitch_real_des, yaw_real_des; - if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, ctrl_pitch_.joint_urdf_)) + if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pos_ctrl_pitch_.joint_urdf_)) { double yaw_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->upper : 1e16; - lower_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->lower : -1e16; + upper_limit = pos_ctrl_pitch_.joint_urdf_->limits ? pos_ctrl_pitch_.joint_urdf_->limits->upper : 1e16; + lower_limit = pos_ctrl_pitch_.joint_urdf_->limits ? pos_ctrl_pitch_.joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, lower_limit)) ? @@ -176,13 +197,13 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_temp); } - if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, ctrl_yaw_.joint_urdf_)) + if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, pos_ctrl_yaw_.joint_urdf_)) { double pitch_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->upper : 1e16; - lower_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->lower : -1e16; + upper_limit = pos_ctrl_yaw_.joint_urdf_->limits ? pos_ctrl_yaw_.joint_urdf_->limits->upper : 1e16; + lower_limit = pos_ctrl_yaw_.joint_urdf_->limits ? pos_ctrl_yaw_.joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, base2gimbal_current_des_pitch, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, lower_limit)) ? @@ -326,10 +347,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { tf2::doTransform(gyro, angular_vel_pitch, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(pos_ctrl_pitch_.joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); tf2::doTransform(gyro, angular_vel_yaw, - robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(pos_ctrl_yaw_.joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); } catch (tf2::TransformException& ex) @@ -340,20 +361,20 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else { - angular_vel_yaw.z = ctrl_yaw_.joint_.getVelocity(); - angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); + angular_vel_yaw.z = pos_ctrl_yaw_.joint_.getVelocity(); + angular_vel_pitch.y = pos_ctrl_pitch_.joint_.getVelocity(); } geometry_msgs::TransformStamped base_frame2des; base_frame2des = - robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); + robot_state_handle_.lookupTransform(pos_ctrl_yaw_.joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); double roll_des, pitch_des, yaw_des; // desired position quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des); - double yaw_vel_des = 0., pitch_vel_des = 0.; + // double yaw_vel_des = 0., pitch_vel_des = 0.; if (state_ == RATE) { - yaw_vel_des = cmd_gimbal_.rate_yaw; - pitch_vel_des = cmd_gimbal_.rate_pitch; + // yaw_vel_des = cmd_gimbal_.rate_yaw; + // pitch_vel_des = cmd_gimbal_.rate_pitch; } else if (state_ == TRACK) { @@ -364,20 +385,20 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + pos_ctrl_yaw_.joint_urdf_->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); - yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, + // yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); + transform = robot_state_handle_.lookupTransform(pos_ctrl_pitch_.joint_urdf_->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); - pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); + // pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); } catch (tf2::TransformException& ex) { @@ -385,27 +406,35 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } } - ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); - ctrl_yaw_.update(time, period); - ctrl_pitch_.update(time, period); - ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z()); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); + // desire vel + double yaw_vel_cmd = pos_pid_yaw_.computeCommand((yaw_des - pos_ctrl_yaw_.joint_.getPosition()), period); + // command effort + double yaw_cmd = vel_pid_yaw_.computeCommand(yaw_vel_cmd - angular_vel_yaw.z, period); + vel_ctrl_yaw_.joint_.setCommand(yaw_cmd); + // vel_pid_yaw_.setCurrentCmd(yaw_cmd); + + double pitch_vel_cmd = pos_pid_pitch_.computeCommand((pitch_des - pos_ctrl_pitch_.joint_.getPosition()), period); + double pitch_cmd = vel_pid_pitch_.computeCommand(pitch_vel_cmd - angular_vel_pitch.y, period); + vel_ctrl_pitch_.joint_.setCommand(pitch_cmd); + // vel_pid_pitch_.setCurrentCmd(pitch_cmd); + + pos_ctrl_yaw_.joint_.setCommand(pos_ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z()); + pos_ctrl_pitch_.joint_.setCommand(pos_ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) { Eigen::Vector3d gravity(0, 0, -gravity_); tf2::doTransform(gravity, gravity, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, "odom", time)); + robot_state_handle_.lookupTransform(pos_ctrl_pitch_.joint_urdf_->child_link_name, "odom", time)); Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); double feedforward = -mass_origin.cross(gravity).y(); if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); tf2::doTransform(gravity_compensation, gravity_compensation, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, - ctrl_pitch_.joint_urdf_->parent_link_name, time)); + robot_state_handle_.lookupTransform(pos_ctrl_pitch_.joint_urdf_->child_link_name, + pos_ctrl_pitch_.joint_urdf_->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; From 3abc5d28e5edd6c5815ac012242ee2df47c9853d Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 12:45:12 +0800 Subject: [PATCH 02/27] Modify way of getting joint urdf and use cascade pid. --- .../rm_gimbal_controllers/gimbal_base.h | 7 +- rm_gimbal_controllers/src/gimbal_base.cpp | 83 ++++++++++++------- 2 files changed, 59 insertions(+), 31 deletions(-) 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 003d9dca..1e2eb626 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -38,6 +38,7 @@ #pragma once #include +#include #include #include #include @@ -51,6 +52,8 @@ #include #include #include +#include +#include namespace rm_gimbal_controllers { @@ -171,7 +174,8 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; @@ -182,6 +186,7 @@ class Controller : public controller_interface::MultiInterfaceController cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; + urdf::JointConstSharedPtr pitch_joint_urdf_, yaw_joint_urdf_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index ae9a4237..ac791c4c 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -70,10 +70,21 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); + ros::NodeHandle nh_yaw_pos = ros::NodeHandle(controller_nh, "yaw_pos"); + ros::NodeHandle nh_pitch_pos = ros::NodeHandle(controller_nh, "pitch_pos"); + hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) return false; + + if (controller_nh.hasParam("pos_pid_yaw")) + if (!pos_pid_yaw_.init(ros::NodeHandle(controller_nh, "pos_pid_yaw"))) + return false; + if (controller_nh.hasParam("pos_pid_pitch")) + if (!pos_pid_pitch_.init(ros::NodeHandle(controller_nh, "pos_pid_pitch"))) + return false; + robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); if (!controller_nh.hasParam("imu_name")) has_imu_ = false; @@ -89,15 +100,35 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - gimbal_des_frame_id_ = ctrl_pitch_.joint_urdf_->child_link_name + "_des"; + // Get URDF info about joint + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + pitch_joint_urdf_ = urdf.getJoint("pitch"); + yaw_joint_urdf_ = urdf.getJoint("yaw"); + if (!pitch_joint_urdf_) + { + ROS_ERROR("Could not find joint pitch in urdf"); + return false; + } + if (!yaw_joint_urdf_) + { + ROS_ERROR("Could not find joint yaw in urdf"); + return false; + } + + gimbal_des_frame_id_ = pitch_joint_urdf_->child_link_name + "_des"; odom2gimbal_des_.header.frame_id = "odom"; odom2gimbal_des_.child_frame_id = gimbal_des_frame_id_; odom2gimbal_des_.transform.rotation.w = 1.; odom2pitch_.header.frame_id = "odom"; - odom2pitch_.child_frame_id = ctrl_pitch_.joint_urdf_->child_link_name; + odom2pitch_.child_frame_id = pitch_joint_urdf_->child_link_name; odom2pitch_.transform.rotation.w = 1.; odom2base_.header.frame_id = "odom"; - odom2base_.child_frame_id = ctrl_yaw_.joint_urdf_->parent_link_name; + odom2base_.child_frame_id = yaw_joint_urdf_->parent_link_name; odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); @@ -120,8 +151,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) data_track_ = *track_rt_buffer_.readFromNonRT(); try { - odom2pitch_ = robot_state_handle_.lookupTransform("odom", ctrl_pitch_.joint_urdf_->child_link_name, time); - odom2base_ = robot_state_handle_.lookupTransform("odom", ctrl_yaw_.joint_urdf_->parent_link_name, time); + odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time); + odom2base_ = robot_state_handle_.lookupTransform("odom", yaw_joint_urdf_->parent_link_name, time); } catch (tf2::TransformException& ex) { @@ -160,13 +191,13 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(base2gimbal_des), roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw); double pitch_real_des, yaw_real_des; - if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, ctrl_pitch_.joint_urdf_)) + if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pitch_joint_urdf_)) { double yaw_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->upper : 1e16; - lower_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->lower : -1e16; + upper_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->upper : 1e16; + lower_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, lower_limit)) ? @@ -176,13 +207,13 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_temp); } - if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, ctrl_yaw_.joint_urdf_)) + if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, yaw_joint_urdf_)) { double pitch_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->upper : 1e16; - lower_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->lower : -1e16; + upper_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->upper : 1e16; + lower_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, base2gimbal_current_des_pitch, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, lower_limit)) ? @@ -326,10 +357,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { tf2::doTransform(gyro, angular_vel_pitch, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); tf2::doTransform(gyro, angular_vel_yaw, - robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(yaw_joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); } catch (tf2::TransformException& ex) @@ -344,8 +375,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); } geometry_msgs::TransformStamped base_frame2des; - base_frame2des = - robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); + base_frame2des = robot_state_handle_.lookupTransform(yaw_joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); double roll_des, pitch_des, yaw_des; // desired position quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des); @@ -364,48 +394,41 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + yaw_joint_urdf_->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); - yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); + // yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); + transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->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); - pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); + // pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); } } - - ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); - ctrl_yaw_.update(time, period); - ctrl_pitch_.update(time, period); - ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z()); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) { Eigen::Vector3d gravity(0, 0, -gravity_); tf2::doTransform(gravity, gravity, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, "odom", time)); + robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, "odom", time)); Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); double feedforward = -mass_origin.cross(gravity).y(); if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); tf2::doTransform(gravity_compensation, gravity_compensation, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, - ctrl_pitch_.joint_urdf_->parent_link_name, time)); + robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, + pitch_joint_urdf_->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; From a7815f7a3f31662b52102e8c8eb5e1b583d19239 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 12:51:09 +0800 Subject: [PATCH 03/27] Fix some merge problems. --- rm_gimbal_controllers/src/gimbal_base.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index dde70654..865fe9b2 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -414,8 +414,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::fromMsg(target_vel, target_vel_tf); yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); + transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->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); @@ -428,8 +428,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } } - ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); + // ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); + // ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); double resistance_compensation = 0.; From 6c4f4443c6f4029356d86ad734a9d0268bc387c6 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 13:21:47 +0800 Subject: [PATCH 04/27] Modify ways of computing pitch and yaw vel des. --- .../rm_gimbal_controllers/gimbal_base.h | 2 + rm_gimbal_controllers/src/gimbal_base.cpp | 61 +++++-------------- 2 files changed, 18 insertions(+), 45 deletions(-) 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 8e1efbb4..b7677520 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -54,6 +54,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -148,6 +149,7 @@ class Controller : public controller_interface::MultiInterfaceController pitch_vel_des_filter_, yaw_vel_des_filter_; std::shared_ptr bullet_solver_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 865fe9b2..08bde0a4 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -143,6 +144,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); + pitch_vel_des_filter_ = std::make_shared(nh_pitch); + yaw_vel_des_filter_ = std::make_shared(nh_yaw); + return true; } @@ -384,64 +388,31 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_yaw.z = ctrl_yaw_.joint_.getVelocity(); angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); } - geometry_msgs::TransformStamped base_frame2des; - base_frame2des = robot_state_handle_.lookupTransform(yaw_joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); - double roll_des, pitch_des, yaw_des; // desired position - quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des); double yaw_vel_des = 0., pitch_vel_des = 0.; if (state_ == RATE) { yaw_vel_des = cmd_gimbal_.rate_yaw; pitch_vel_des = cmd_gimbal_.rate_pitch; + double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; + quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); + quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); + yaw_vel_des = angles::shortest_angular_distance(yaw_real, yaw_des) / period.toSec(); + pitch_vel_des = angles::shortest_angular_distance(pitch_real, pitch_des) / period.toSec(); } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, data_track_.position, data_track_.velocity, - data_track_.yaw, data_track_.v_yaw, data_track_.radius_1, - data_track_.radius_2, data_track_.dz, data_track_.armors_num); - tf2::Vector3 target_pos_tf, target_vel_tf; - - try - { - geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - yaw_joint_urdf_->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); - - yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->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); - pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - } + double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; + quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); + quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); + yaw_vel_des = angles::shortest_angular_distance(yaw_real, yaw_des) / period.toSec(); + pitch_vel_des = angles::shortest_angular_distance(pitch_real, pitch_des) / period.toSec(); } - // ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - // ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); + ctrl_yaw_.setCommand(yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); + ctrl_pitch_.setCommand(pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - double resistance_compensation = 0.; - if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_saturation_point_) - resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; - else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_saturation_point_) - resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; - else - resistance_compensation = ctrl_yaw_.joint_.getCommand() * yaw_resistance_ / effort_saturation_point_; - ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + - yaw_k_v_ * yaw_vel_des + resistance_compensation); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des); } double Controller::feedForward(const ros::Time& time) From b847df993149c324d34dc5d9059e72d650077de9 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 13:40:09 +0800 Subject: [PATCH 05/27] Fix a get param bug. --- rm_gimbal_controllers/src/gimbal_base.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 08bde0a4..c6cc37ae 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -115,8 +115,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_ERROR("Failed to parse urdf file"); return false; } - pitch_joint_urdf_ = urdf.getJoint("pitch"); - yaw_joint_urdf_ = urdf.getJoint("yaw"); + pitch_joint_urdf_ = urdf.getJoint(ctrl_pitch_.getJointName()); + yaw_joint_urdf_ = urdf.getJoint(ctrl_yaw_.getJointName()); if (!pitch_joint_urdf_) { ROS_ERROR("Could not find joint pitch in urdf"); @@ -394,11 +394,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { yaw_vel_des = cmd_gimbal_.rate_yaw; pitch_vel_des = cmd_gimbal_.rate_pitch; - double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; - quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); - quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - yaw_vel_des = angles::shortest_angular_distance(yaw_real, yaw_des) / period.toSec(); - pitch_vel_des = angles::shortest_angular_distance(pitch_real, pitch_des) / period.toSec(); } else if (state_ == TRACK) { From 35ff34014adda74f96e39cfa0a338a4065e17857 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 13:58:15 +0800 Subject: [PATCH 06/27] Modify ways of computing gimbal vel des. --- .../rm_gimbal_controllers/gimbal_base.h | 29 +++++++++++++++++-- rm_gimbal_controllers/src/gimbal_base.cpp | 20 ++++++++----- 2 files changed, 39 insertions(+), 10 deletions(-) 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 b7677520..1d9a259b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -58,6 +58,31 @@ namespace rm_gimbal_controllers { +class GimbalDesVel +{ +public: + GimbalDesVel(ros::NodeHandle& nh) + { + ros::NodeHandle nh_yaw = ros::NodeHandle(nh, "yaw"); + ros::NodeHandle nh_pitch = ros::NodeHandle(nh, "pitch"); + pitch_vel_des_lp_filter_ = std::make_shared(nh_pitch); + yaw_vel_des_lp_filter_ = std::make_shared(nh_yaw); + } + std::shared_ptr pitch_vel_des_lp_filter_, yaw_vel_des_lp_filter_; + void update(double yaw_vel_des, double pitch_vel_des, double period, const ros::Time& time) + { + if (period < 0) + return; + if (period > 0.1) + { + pitch_vel_des_lp_filter_->reset(); + yaw_vel_des_lp_filter_->reset(); + } + pitch_vel_des_lp_filter_->input(pitch_vel_des, time); + yaw_vel_des_lp_filter_->input(yaw_vel_des, time); + } +}; + class ChassisVel { public: @@ -149,9 +174,9 @@ class Controller : public controller_interface::MultiInterfaceController pitch_vel_des_filter_, yaw_vel_des_filter_; std::shared_ptr bullet_solver_; + std::shared_ptr gimbal_des_vel_; // ROS Interface ros::Time last_publish_time_{}; @@ -169,7 +194,7 @@ class Controller : public controller_interface::MultiInterfaceController(gimbal_des_vel_nh); + hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) @@ -144,9 +148,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - pitch_vel_des_filter_ = std::make_shared(nh_pitch); - yaw_vel_des_filter_ = std::make_shared(nh_yaw); - return true; } @@ -397,11 +398,14 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; - quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); - quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - yaw_vel_des = angles::shortest_angular_distance(yaw_real, yaw_des) / period.toSec(); - pitch_vel_des = angles::shortest_angular_distance(pitch_real, pitch_des) / period.toSec(); + double tf_period = odom2gimbal_des_.header.stamp.toSec() - last_odom2gimbal_des_.header.stamp.toSec(); + double last_roll, last_pitch, last_yaw, roll, pitch, yaw; + quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); + quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll, last_pitch, last_yaw); + yaw_vel_des = angles::shortest_angular_distance(last_yaw, yaw) / tf_period; + pitch_vel_des = angles::shortest_angular_distance(last_pitch, pitch) / tf_period; + gimbal_des_vel_->update(yaw_vel_des, pitch_vel_des, tf_period, time); + last_odom2gimbal_des_ = odom2gimbal_des_; } ctrl_yaw_.setCommand(yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); From a89bcd4071c4f748afd2a739cd99938368872724 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 14:00:26 +0800 Subject: [PATCH 07/27] Fix a bug of using variables. --- rm_gimbal_controllers/src/gimbal_base.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index d8de23dd..3bbc2e6e 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -402,9 +402,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) double last_roll, last_pitch, last_yaw, roll, pitch, yaw; quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll, last_pitch, last_yaw); - yaw_vel_des = angles::shortest_angular_distance(last_yaw, yaw) / tf_period; - pitch_vel_des = angles::shortest_angular_distance(last_pitch, pitch) / tf_period; - gimbal_des_vel_->update(yaw_vel_des, pitch_vel_des, tf_period, time); + double yaw_vel = angles::shortest_angular_distance(last_yaw, yaw) / tf_period; + double pitch_vel = angles::shortest_angular_distance(last_pitch, pitch) / tf_period; + gimbal_des_vel_->update(yaw_vel, pitch_vel, tf_period, time); + yaw_vel_des = gimbal_des_vel_->yaw_vel_des_lp_filter_->output(); + pitch_vel_des = gimbal_des_vel_->pitch_vel_des_lp_filter_->output(); last_odom2gimbal_des_ = odom2gimbal_des_; } From f74823ad031cbaa78909856f7ba93b8327b4951f Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 17:17:36 +0800 Subject: [PATCH 08/27] Add position loop and simplify some logic. --- rm_gimbal_controllers/src/gimbal_base.cpp | 36 +++++++++++++++++------ 1 file changed, 27 insertions(+), 9 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 3bbc2e6e..e940f802 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -82,7 +82,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_yaw_pos = ros::NodeHandle(controller_nh, "yaw_pos"); ros::NodeHandle nh_pitch_pos = ros::NodeHandle(controller_nh, "pitch_pos"); - ros::NodeHandle gimbal_des_vel_nh(controller_nh, "yaw_des_vel"); + ros::NodeHandle gimbal_des_vel_nh(controller_nh, "gimbal_des_vel"); gimbal_des_vel_ = std::make_shared(gimbal_des_vel_nh); hardware_interface::EffortJointInterface* effort_joint_interface = @@ -391,27 +391,45 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } double yaw_vel_des = 0., pitch_vel_des = 0.; + double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; + quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); if (state_ == RATE) { yaw_vel_des = cmd_gimbal_.rate_yaw; pitch_vel_des = cmd_gimbal_.rate_pitch; + + double tf_period = odom2gimbal_des_.header.stamp.toSec() - last_odom2gimbal_des_.header.stamp.toSec(); + double last_roll_des, last_pitch_des, last_yaw_des; + quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll_des, last_pitch_des, last_yaw_des); + double yaw_vel = angles::shortest_angular_distance(last_yaw_des, yaw_des) / tf_period; + double pitch_vel = angles::shortest_angular_distance(last_pitch_des, pitch_des) / tf_period; + gimbal_des_vel_->update(yaw_vel, pitch_vel, tf_period, time); + yaw_vel_des = gimbal_des_vel_->yaw_vel_des_lp_filter_->output(); + pitch_vel_des = gimbal_des_vel_->pitch_vel_des_lp_filter_->output(); + last_odom2gimbal_des_ = odom2gimbal_des_; } - else if (state_ == TRACK) + else { double tf_period = odom2gimbal_des_.header.stamp.toSec() - last_odom2gimbal_des_.header.stamp.toSec(); - double last_roll, last_pitch, last_yaw, roll, pitch, yaw; - quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); - quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll, last_pitch, last_yaw); - double yaw_vel = angles::shortest_angular_distance(last_yaw, yaw) / tf_period; - double pitch_vel = angles::shortest_angular_distance(last_pitch, pitch) / tf_period; + double last_roll_des, last_pitch_des, last_yaw_des; + quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll_des, last_pitch_des, last_yaw_des); + double yaw_vel = angles::shortest_angular_distance(last_yaw_des, yaw_des) / tf_period; + double pitch_vel = angles::shortest_angular_distance(last_pitch_des, pitch_des) / tf_period; gimbal_des_vel_->update(yaw_vel, pitch_vel, tf_period, time); yaw_vel_des = gimbal_des_vel_->yaw_vel_des_lp_filter_->output(); pitch_vel_des = gimbal_des_vel_->pitch_vel_des_lp_filter_->output(); last_odom2gimbal_des_ = odom2gimbal_des_; } - ctrl_yaw_.setCommand(yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); + quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); + double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); + double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); + pos_pid_pitch_.computeCommand(pitch_angle_error, period); + pos_pid_yaw_.computeCommand(yaw_angle_error, period); + + ctrl_yaw_.setCommand(pos_pid_yaw_.getCurrentCmd() + yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); + ctrl_pitch_.setCommand(pos_pid_pitch_.getCurrentCmd() + pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - + angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); } From e2794072c28944a9ae7bff6a6f2e75ee9057ba1b Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 18:59:12 +0800 Subject: [PATCH 09/27] Publish position loop state. --- .../rm_gimbal_controllers/gimbal_base.h | 3 + rm_gimbal_controllers/src/gimbal_base.cpp | 58 +++++++++++-------- 2 files changed, 38 insertions(+), 23 deletions(-) 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 1d9a259b..c4484e79 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -177,6 +177,8 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; std::shared_ptr gimbal_des_vel_; + std::unique_ptr> pos_pid_yaw_pub_, + pos_pid_pitch_pub_; // ROS Interface ros::Time last_publish_time_{}; @@ -192,6 +194,7 @@ class Controller : public controller_interface::MultiInterfaceController(gimbal_des_vel_nh); + ros::NodeHandle nh_yaw_pos = ros::NodeHandle(controller_nh, "yaw_pos"); + ros::NodeHandle nh_pitch_pos = ros::NodeHandle(controller_nh, "pitch_pos"); + pos_pid_yaw_pub_.reset( + new realtime_tools::RealtimePublisher(nh_yaw_pos, "state", 1)); + pos_pid_pitch_pub_.reset( + new realtime_tools::RealtimePublisher(nh_pitch_pos, "state", 1)); + if (!pos_pid_yaw_.init(nh_yaw_pos) || !pos_pid_pitch_.init(nh_pitch_pos)) + return false; + hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) return false; - if (controller_nh.hasParam("pos_pid_yaw")) - if (!pos_pid_yaw_.init(ros::NodeHandle(controller_nh, "pos_pid_yaw"))) - return false; - if (controller_nh.hasParam("pos_pid_pitch")) - if (!pos_pid_pitch_.init(ros::NodeHandle(controller_nh, "pos_pid_pitch"))) - return false; - robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); if (!controller_nh.hasParam("imu_name")) has_imu_ = false; @@ -397,16 +396,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { yaw_vel_des = cmd_gimbal_.rate_yaw; pitch_vel_des = cmd_gimbal_.rate_pitch; - - double tf_period = odom2gimbal_des_.header.stamp.toSec() - last_odom2gimbal_des_.header.stamp.toSec(); - double last_roll_des, last_pitch_des, last_yaw_des; - quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll_des, last_pitch_des, last_yaw_des); - double yaw_vel = angles::shortest_angular_distance(last_yaw_des, yaw_des) / tf_period; - double pitch_vel = angles::shortest_angular_distance(last_pitch_des, pitch_des) / tf_period; - gimbal_des_vel_->update(yaw_vel, pitch_vel, tf_period, time); - yaw_vel_des = gimbal_des_vel_->yaw_vel_des_lp_filter_->output(); - pitch_vel_des = gimbal_des_vel_->pitch_vel_des_lp_filter_->output(); - last_odom2gimbal_des_ = odom2gimbal_des_; } else { @@ -427,9 +416,32 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) pos_pid_pitch_.computeCommand(pitch_angle_error, period); pos_pid_yaw_.computeCommand(yaw_angle_error, period); - ctrl_yaw_.setCommand(pos_pid_yaw_.getCurrentCmd() + yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pos_pid_pitch_.getCurrentCmd() + pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - - angular_vel_pitch.y); + // publish state + if (loop_count_ % 10 == 0) + { + if (pos_pid_yaw_pub_ && pos_pid_yaw_pub_->trylock()) + { + pos_pid_yaw_pub_->msg_.header.stamp = time; + pos_pid_yaw_pub_->msg_.set_point = yaw_des; + pos_pid_yaw_pub_->msg_.process_value = yaw_real; + pos_pid_yaw_pub_->msg_.process_value_dot = ctrl_yaw_.joint_.getVelocity(); + pos_pid_yaw_pub_->msg_.error = yaw_angle_error; + pos_pid_yaw_pub_->msg_.time_step = period.toSec(); + pos_pid_yaw_pub_->msg_.command = pos_pid_yaw_.getCurrentCmd(); + double dummy; + bool antiwindup; + pos_pid_yaw_.getGains(pos_pid_yaw_pub_->msg_.p, pos_pid_yaw_pub_->msg_.i, pos_pid_yaw_pub_->msg_.d, + pos_pid_yaw_pub_->msg_.i_clamp, dummy, antiwindup); + pos_pid_yaw_pub_->msg_.antiwindup = static_cast(antiwindup); + pos_pid_yaw_pub_->unlockAndPublish(); + } + } + loop_count_++; + + ctrl_yaw_.setCommand(pos_pid_yaw_.getCurrentCmd() + yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - + angular_vel_yaw.z); + ctrl_pitch_.setCommand(pos_pid_pitch_.getCurrentCmd() + pitch_k_v_ * pitch_vel_des + + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); } From ea296f1fd037097eb70c72150b85099fb1adc3f1 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 22:53:47 +0800 Subject: [PATCH 10/27] Publish pitch position loop state. --- rm_gimbal_controllers/src/gimbal_base.cpp | 25 ++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 3fc8143a..b4ba71f7 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -84,10 +84,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_yaw_pos = ros::NodeHandle(controller_nh, "yaw_pos"); ros::NodeHandle nh_pitch_pos = ros::NodeHandle(controller_nh, "pitch_pos"); - pos_pid_yaw_pub_.reset( - new realtime_tools::RealtimePublisher(nh_yaw_pos, "state", 1)); - pos_pid_pitch_pub_.reset( - new realtime_tools::RealtimePublisher(nh_pitch_pos, "state", 1)); if (!pos_pid_yaw_.init(nh_yaw_pos) || !pos_pid_pitch_.init(nh_pitch_pos)) return false; @@ -146,7 +142,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - + pos_pid_yaw_pub_.reset( + new realtime_tools::RealtimePublisher(nh_yaw_pos, "state", 1)); + pos_pid_pitch_pub_.reset( + new realtime_tools::RealtimePublisher(nh_pitch_pos, "state", 1)); return true; } @@ -435,6 +434,22 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) pos_pid_yaw_pub_->msg_.antiwindup = static_cast(antiwindup); pos_pid_yaw_pub_->unlockAndPublish(); } + if (pos_pid_pitch_pub_ && pos_pid_pitch_pub_->trylock()) + { + pos_pid_pitch_pub_->msg_.header.stamp = time; + pos_pid_pitch_pub_->msg_.set_point = pitch_des; + pos_pid_pitch_pub_->msg_.process_value = pitch_real; + pos_pid_pitch_pub_->msg_.process_value_dot = ctrl_pitch_.joint_.getVelocity(); + pos_pid_pitch_pub_->msg_.error = pitch_angle_error; + pos_pid_pitch_pub_->msg_.time_step = period.toSec(); + pos_pid_pitch_pub_->msg_.command = pos_pid_yaw_.getCurrentCmd(); + double dummy; + bool antiwindup; + pos_pid_pitch_.getGains(pos_pid_pitch_pub_->msg_.p, pos_pid_pitch_pub_->msg_.i, pos_pid_pitch_pub_->msg_.d, + pos_pid_pitch_pub_->msg_.i_clamp, dummy, antiwindup); + pos_pid_pitch_pub_->msg_.antiwindup = static_cast(antiwindup); + pos_pid_pitch_pub_->unlockAndPublish(); + } } loop_count_++; From d53710e48f519008c5e833f8503b57d7c552a892 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 23:04:14 +0800 Subject: [PATCH 11/27] Modify variable name. --- .../rm_gimbal_controllers/gimbal_base.h | 6 +- rm_gimbal_controllers/src/gimbal_base.cpp | 71 ++++++++++--------- 2 files changed, 39 insertions(+), 38 deletions(-) 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 c4484e79..ab12af8c 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -173,15 +173,15 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; std::shared_ptr gimbal_des_vel_; - std::unique_ptr> pos_pid_yaw_pub_, - pos_pid_pitch_pub_; // ROS Interface ros::Time last_publish_time_{}; + std::unique_ptr> pid_yaw_pos_state_pub_, + pid_pitch_pos_state_pub_; std::shared_ptr> error_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index b4ba71f7..de626e60 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -82,9 +82,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle gimbal_des_vel_nh(controller_nh, "gimbal_des_vel"); gimbal_des_vel_ = std::make_shared(gimbal_des_vel_nh); - ros::NodeHandle nh_yaw_pos = ros::NodeHandle(controller_nh, "yaw_pos"); - ros::NodeHandle nh_pitch_pos = ros::NodeHandle(controller_nh, "pitch_pos"); - if (!pos_pid_yaw_.init(nh_yaw_pos) || !pos_pid_pitch_.init(nh_pitch_pos)) + ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); + ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); + if (!pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) return false; hardware_interface::EffortJointInterface* effort_joint_interface = @@ -142,10 +142,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - pos_pid_yaw_pub_.reset( - new realtime_tools::RealtimePublisher(nh_yaw_pos, "state", 1)); - pos_pid_pitch_pub_.reset( - new realtime_tools::RealtimePublisher(nh_pitch_pos, "state", 1)); + pid_yaw_pos_state_pub_.reset( + new realtime_tools::RealtimePublisher(nh_pid_yaw_pos, "state", 1)); + pid_pitch_pos_state_pub_.reset( + new realtime_tools::RealtimePublisher(nh_pid_pitch_pos, "state", 1)); return true; } @@ -412,50 +412,51 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); - pos_pid_pitch_.computeCommand(pitch_angle_error, period); - pos_pid_yaw_.computeCommand(yaw_angle_error, period); + pid_pitch_pos_.computeCommand(pitch_angle_error, period); + pid_yaw_pos_.computeCommand(yaw_angle_error, period); // publish state if (loop_count_ % 10 == 0) { - if (pos_pid_yaw_pub_ && pos_pid_yaw_pub_->trylock()) + if (pid_yaw_pos_state_pub_ && pid_yaw_pos_state_pub_->trylock()) { - pos_pid_yaw_pub_->msg_.header.stamp = time; - pos_pid_yaw_pub_->msg_.set_point = yaw_des; - pos_pid_yaw_pub_->msg_.process_value = yaw_real; - pos_pid_yaw_pub_->msg_.process_value_dot = ctrl_yaw_.joint_.getVelocity(); - pos_pid_yaw_pub_->msg_.error = yaw_angle_error; - pos_pid_yaw_pub_->msg_.time_step = period.toSec(); - pos_pid_yaw_pub_->msg_.command = pos_pid_yaw_.getCurrentCmd(); + pid_yaw_pos_state_pub_->msg_.header.stamp = time; + pid_yaw_pos_state_pub_->msg_.set_point = yaw_des; + pid_yaw_pos_state_pub_->msg_.process_value = yaw_real; + pid_yaw_pos_state_pub_->msg_.process_value_dot = ctrl_yaw_.joint_.getVelocity(); + pid_yaw_pos_state_pub_->msg_.error = yaw_angle_error; + pid_yaw_pos_state_pub_->msg_.time_step = period.toSec(); + pid_yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); double dummy; bool antiwindup; - pos_pid_yaw_.getGains(pos_pid_yaw_pub_->msg_.p, pos_pid_yaw_pub_->msg_.i, pos_pid_yaw_pub_->msg_.d, - pos_pid_yaw_pub_->msg_.i_clamp, dummy, antiwindup); - pos_pid_yaw_pub_->msg_.antiwindup = static_cast(antiwindup); - pos_pid_yaw_pub_->unlockAndPublish(); + pid_yaw_pos_.getGains(pid_yaw_pos_state_pub_->msg_.p, pid_yaw_pos_state_pub_->msg_.i, + pid_yaw_pos_state_pub_->msg_.d, pid_yaw_pos_state_pub_->msg_.i_clamp, dummy, antiwindup); + pid_yaw_pos_state_pub_->msg_.antiwindup = static_cast(antiwindup); + pid_yaw_pos_state_pub_->unlockAndPublish(); } - if (pos_pid_pitch_pub_ && pos_pid_pitch_pub_->trylock()) + if (pid_pitch_pos_state_pub_ && pid_pitch_pos_state_pub_->trylock()) { - pos_pid_pitch_pub_->msg_.header.stamp = time; - pos_pid_pitch_pub_->msg_.set_point = pitch_des; - pos_pid_pitch_pub_->msg_.process_value = pitch_real; - pos_pid_pitch_pub_->msg_.process_value_dot = ctrl_pitch_.joint_.getVelocity(); - pos_pid_pitch_pub_->msg_.error = pitch_angle_error; - pos_pid_pitch_pub_->msg_.time_step = period.toSec(); - pos_pid_pitch_pub_->msg_.command = pos_pid_yaw_.getCurrentCmd(); + pid_pitch_pos_state_pub_->msg_.header.stamp = time; + pid_pitch_pos_state_pub_->msg_.set_point = pitch_des; + pid_pitch_pos_state_pub_->msg_.process_value = pitch_real; + pid_pitch_pos_state_pub_->msg_.process_value_dot = ctrl_pitch_.joint_.getVelocity(); + pid_pitch_pos_state_pub_->msg_.error = pitch_angle_error; + pid_pitch_pos_state_pub_->msg_.time_step = period.toSec(); + pid_pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); double dummy; bool antiwindup; - pos_pid_pitch_.getGains(pos_pid_pitch_pub_->msg_.p, pos_pid_pitch_pub_->msg_.i, pos_pid_pitch_pub_->msg_.d, - pos_pid_pitch_pub_->msg_.i_clamp, dummy, antiwindup); - pos_pid_pitch_pub_->msg_.antiwindup = static_cast(antiwindup); - pos_pid_pitch_pub_->unlockAndPublish(); + pid_pitch_pos_.getGains(pid_pitch_pos_state_pub_->msg_.p, pid_pitch_pos_state_pub_->msg_.i, + pid_pitch_pos_state_pub_->msg_.d, pid_pitch_pos_state_pub_->msg_.i_clamp, dummy, + antiwindup); + pid_pitch_pos_state_pub_->msg_.antiwindup = static_cast(antiwindup); + pid_pitch_pos_state_pub_->unlockAndPublish(); } } loop_count_++; - ctrl_yaw_.setCommand(pos_pid_yaw_.getCurrentCmd() + yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - + ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() + yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pos_pid_pitch_.getCurrentCmd() + pitch_k_v_ * pitch_vel_des + + ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); From b18abe443b8f000c8ec271f650957e25621d6704 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 19 Mar 2024 23:35:31 +0800 Subject: [PATCH 12/27] Modify topic name and add pitch gravity feedforward. --- rm_gimbal_controllers/src/gimbal_base.cpp | 35 +++++++++-------------- 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index de626e60..e74699ae 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -142,10 +142,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - pid_yaw_pos_state_pub_.reset( - new realtime_tools::RealtimePublisher(nh_pid_yaw_pos, "state", 1)); - pid_pitch_pos_state_pub_.reset( - new realtime_tools::RealtimePublisher(nh_pid_pitch_pos, "state", 1)); + pid_yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher( + nh_pid_yaw_pos, "pid_yaw_pos_state", 1)); + pid_pitch_pos_state_pub_.reset(new realtime_tools::RealtimePublisher( + nh_pid_pitch_pos, "pid_pitch_pos_state", 1)); return true; } @@ -391,23 +391,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) double yaw_vel_des = 0., pitch_vel_des = 0.; double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - if (state_ == RATE) - { - yaw_vel_des = cmd_gimbal_.rate_yaw; - pitch_vel_des = cmd_gimbal_.rate_pitch; - } - else - { - double tf_period = odom2gimbal_des_.header.stamp.toSec() - last_odom2gimbal_des_.header.stamp.toSec(); - double last_roll_des, last_pitch_des, last_yaw_des; - quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll_des, last_pitch_des, last_yaw_des); - double yaw_vel = angles::shortest_angular_distance(last_yaw_des, yaw_des) / tf_period; - double pitch_vel = angles::shortest_angular_distance(last_pitch_des, pitch_des) / tf_period; - gimbal_des_vel_->update(yaw_vel, pitch_vel, tf_period, time); - yaw_vel_des = gimbal_des_vel_->yaw_vel_des_lp_filter_->output(); - pitch_vel_des = gimbal_des_vel_->pitch_vel_des_lp_filter_->output(); - last_odom2gimbal_des_ = odom2gimbal_des_; - } + double tf_period = odom2gimbal_des_.header.stamp.toSec() - last_odom2gimbal_des_.header.stamp.toSec(); + double last_roll_des, last_pitch_des, last_yaw_des; + quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll_des, last_pitch_des, last_yaw_des); + double yaw_vel = angles::shortest_angular_distance(last_yaw_des, yaw_des) / tf_period; + double pitch_vel = angles::shortest_angular_distance(last_pitch_des, pitch_des) / tf_period; + gimbal_des_vel_->update(yaw_vel, pitch_vel, tf_period, time); + yaw_vel_des = gimbal_des_vel_->yaw_vel_des_lp_filter_->output(); + pitch_vel_des = gimbal_des_vel_->pitch_vel_des_lp_filter_->output(); + last_odom2gimbal_des_ = odom2gimbal_des_; quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); @@ -458,6 +450,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_yaw.z); ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); } From 88ef3df76539ef8dbdaadbe1721295d51503f214 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Wed, 20 Mar 2024 13:46:13 +0800 Subject: [PATCH 13/27] Add output limit. --- .../include/rm_gimbal_controllers/gimbal_base.h | 1 + rm_gimbal_controllers/src/gimbal_base.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+) 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 ab12af8c..b7d398a8 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -174,6 +174,7 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; std::shared_ptr gimbal_des_vel_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e74699ae..4e260fa9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -84,6 +84,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); + if (!nh_pid_yaw_pos.getParam("max_output", max_pid_yaw_pos_output_) || + !nh_pid_pitch_pos.getParam("max_output", max_pid_pitch_pos_output_)) + ROS_ERROR("Pid pos max output has not defined"); if (!pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) return false; @@ -388,6 +391,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); } + // compute gimbal vel des double yaw_vel_des = 0., pitch_vel_des = 0.; double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); @@ -407,6 +411,13 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) pid_pitch_pos_.computeCommand(pitch_angle_error, period); pid_yaw_pos_.computeCommand(yaw_angle_error, period); + // limit feedforward output + if (std::abs(pitch_vel_des) > max_pid_pitch_pos_output_) + pitch_vel_des = pitch_vel_des > 0. ? max_pid_pitch_pos_output_ : -max_pid_pitch_pos_output_; + if (std::abs(pid_pitch_pos_.getCurrentCmd()) > max_pid_yaw_pos_output_) + pid_pitch_pos_.setCurrentCmd(pid_pitch_pos_.getCurrentCmd() > 0. ? max_pid_yaw_pos_output_ : + -max_pid_yaw_pos_output_); + // publish state if (loop_count_ % 10 == 0) { From 072ffdc309edcb6ee29063461f06329156f3e040 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Wed, 20 Mar 2024 18:29:58 +0800 Subject: [PATCH 14/27] Add cfg file for gimbal base. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 rm_gimbal_controllers/cfg/GimbalBase.cfg diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg new file mode 100644 index 00000000..08fc05bf --- /dev/null +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -0,0 +1,11 @@ +#!/usr/bin/env python +PACKAGE = "rm_gimbal_controllers" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("max_pid_yaw_pos_output", double_t, 0, "Pid yaw position max output", 0.0, 0, 10.0) +gen.add("max_pid_pitch_pos_output", double_t, 0, "Pid pitch position max output", 0.0, 0, 10.0) + +exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) From f63848be02d67dac20e50a74f118f92a67a9ed37 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Wed, 20 Mar 2024 18:45:02 +0800 Subject: [PATCH 15/27] Add dynamic reconfig for gimbal base. --- rm_gimbal_controllers/CMakeLists.txt | 3 +- rm_gimbal_controllers/cfg/GimbalBase.cfg | 0 .../rm_gimbal_controllers/gimbal_base.h | 14 +++++- rm_gimbal_controllers/src/gimbal_base.cpp | 45 ++++++++++++++----- 4 files changed, 48 insertions(+), 14 deletions(-) mode change 100644 => 100755 rm_gimbal_controllers/cfg/GimbalBase.cfg diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98732896..e4f8bd33 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -24,10 +24,11 @@ find_package(catkin REQUIRED visualization_msgs dynamic_reconfigure angles - ) +) generate_dynamic_reconfigure_options( cfg/BulletSolver.cfg + cfg/GimbalBase.cfg ) ################################### diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg old mode 100644 new mode 100755 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 b7d398a8..5628b433 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -55,9 +55,16 @@ #include #include #include +#include +#include namespace rm_gimbal_controllers { +struct GimbalConfig +{ + double max_pid_yaw_pos_output, max_pid_pitch_pos_output; +}; + class GimbalDesVel { public: @@ -168,13 +175,13 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; std::shared_ptr gimbal_des_vel_; @@ -217,6 +224,11 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; + bool dynamic_reconfig_initialized_{}; + GimbalConfig config_{}; + realtime_tools::RealtimeBuffer config_rt_buffer_; + dynamic_reconfigure::Server* d_srv_{}; + enum { RATE, diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 4e260fa9..32c6851a 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -43,7 +43,6 @@ #include #include #include -#include namespace rm_gimbal_controllers { @@ -84,15 +83,19 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); - if (!nh_pid_yaw_pos.getParam("max_output", max_pid_yaw_pos_output_) || - !nh_pid_pitch_pos.getParam("max_output", max_pid_pitch_pos_output_)) - ROS_ERROR("Pid pos max output has not defined"); - if (!pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) - return false; + + config_ = { .max_pid_yaw_pos_output = getParam(nh_pid_yaw_pos, "max_output", 0.), + .max_pid_pitch_pos_output = getParam(nh_pid_pitch_pos, "max_output", 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 (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) + if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch) || + !pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) return false; robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); @@ -162,6 +165,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + config_ = *config_rt_buffer_.readFromRT(); try { odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time); @@ -412,11 +416,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) pid_yaw_pos_.computeCommand(yaw_angle_error, period); // limit feedforward output - if (std::abs(pitch_vel_des) > max_pid_pitch_pos_output_) - pitch_vel_des = pitch_vel_des > 0. ? max_pid_pitch_pos_output_ : -max_pid_pitch_pos_output_; - if (std::abs(pid_pitch_pos_.getCurrentCmd()) > max_pid_yaw_pos_output_) - pid_pitch_pos_.setCurrentCmd(pid_pitch_pos_.getCurrentCmd() > 0. ? max_pid_yaw_pos_output_ : - -max_pid_yaw_pos_output_); + if (std::abs(pitch_vel_des) > config_.max_pid_pitch_pos_output) + pitch_vel_des = pitch_vel_des > 0. ? config_.max_pid_pitch_pos_output : -config_.max_pid_pitch_pos_output; + if (std::abs(pid_pitch_pos_.getCurrentCmd()) > config_.max_pid_yaw_pos_output) + pid_pitch_pos_.setCurrentCmd(pid_pitch_pos_.getCurrentCmd() > 0. ? config_.max_pid_yaw_pos_output : + -config_.max_pid_yaw_pos_output); // publish state if (loop_count_ % 10 == 0) @@ -516,6 +520,23 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } +void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) +{ + ROS_INFO("[Gimbal Base] Dynamic params change"); + if (!dynamic_reconfig_initialized_) + { + GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml + config.max_pid_yaw_pos_output = init_config.max_pid_yaw_pos_output; + config.max_pid_pitch_pos_output = init_config.max_pid_pitch_pos_output; + dynamic_reconfig_initialized_ = true; + } + GimbalConfig config_non_rt{ + .max_pid_yaw_pos_output = config.max_pid_yaw_pos_output, + .max_pid_pitch_pos_output = config.max_pid_pitch_pos_output, + }; + config_rt_buffer_.writeFromNonRT(config_non_rt); +} + } // namespace rm_gimbal_controllers PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::Controller, controller_interface::ControllerBase) From 677a5e5fcd02b3aaeadf12489e9223a8cc58aa22 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Wed, 20 Mar 2024 19:16:28 +0800 Subject: [PATCH 16/27] Modify max output limit. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 08fc05bf..ffe9579c 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -5,7 +5,7 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("max_pid_yaw_pos_output", double_t, 0, "Pid yaw position max output", 0.0, 0, 10.0) -gen.add("max_pid_pitch_pos_output", double_t, 0, "Pid pitch position max output", 0.0, 0, 10.0) +gen.add("max_pid_yaw_pos_output", double_t, 0, "Pid yaw position max output", 0.0, 0, 99.0) +gen.add("max_pid_pitch_pos_output", double_t, 0, "Pid pitch position max output", 0.0, 0, 99.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) From 5aa5fe48102a396e3debdb039e355c26ac09c67f Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Wed, 20 Mar 2024 19:28:03 +0800 Subject: [PATCH 17/27] Unlimit pid pos and feedforward output. --- rm_gimbal_controllers/src/gimbal_base.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 32c6851a..c845d11c 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -416,11 +416,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) pid_yaw_pos_.computeCommand(yaw_angle_error, period); // limit feedforward output - if (std::abs(pitch_vel_des) > config_.max_pid_pitch_pos_output) - pitch_vel_des = pitch_vel_des > 0. ? config_.max_pid_pitch_pos_output : -config_.max_pid_pitch_pos_output; - if (std::abs(pid_pitch_pos_.getCurrentCmd()) > config_.max_pid_yaw_pos_output) - pid_pitch_pos_.setCurrentCmd(pid_pitch_pos_.getCurrentCmd() > 0. ? config_.max_pid_yaw_pos_output : - -config_.max_pid_yaw_pos_output); + // if (std::abs(pitch_vel_des) > config_.max_pid_pitch_pos_output) + // pitch_vel_des = pitch_vel_des > 0. ? config_.max_pid_pitch_pos_output : -config_.max_pid_pitch_pos_output; + // if (std::abs(pid_pitch_pos_.getCurrentCmd()) > config_.max_pid_yaw_pos_output) + // pid_pitch_pos_.setCurrentCmd(pid_pitch_pos_.getCurrentCmd() > 0. ? config_.max_pid_yaw_pos_output : + // -config_.max_pid_yaw_pos_output); // publish state if (loop_count_ % 10 == 0) From 90afd2a2e16884d0e5ecf257d83747a5473b0975 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Thu, 21 Mar 2024 16:57:12 +0800 Subject: [PATCH 18/27] Modify param for dynamic reconfig. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 2 ++ .../include/rm_gimbal_controllers/gimbal_base.h | 6 ++---- rm_gimbal_controllers/src/gimbal_base.cpp | 10 +++++----- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index ffe9579c..476392b5 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -7,5 +7,7 @@ gen = ParameterGenerator() gen.add("max_pid_yaw_pos_output", double_t, 0, "Pid yaw position max output", 0.0, 0, 99.0) gen.add("max_pid_pitch_pos_output", double_t, 0, "Pid pitch position max output", 0.0, 0, 99.0) +gen.add("yaw_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) +gen.add("pitch_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) 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 5628b433..1f83a233 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -63,6 +63,8 @@ namespace rm_gimbal_controllers struct GimbalConfig { double max_pid_yaw_pos_output, max_pid_pitch_pos_output; + // Input feedforward + double yaw_k_v_, pitch_k_v_; }; class GimbalDesVel @@ -212,10 +214,6 @@ class Controller : public controller_interface::MultiInterfaceController(gimbal_des_vel_nh); @@ -85,7 +83,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); config_ = { .max_pid_yaw_pos_output = getParam(nh_pid_yaw_pos, "max_output", 0.), - .max_pid_pitch_pos_output = getParam(nh_pid_pitch_pos, "max_output", 0.) }; + .max_pid_pitch_pos_output = getParam(nh_pid_pitch_pos, "max_output", 0.), + .yaw_k_v_ = getParam(nh_pitch, "k_v", 0.), + .pitch_k_v_ = getParam(nh_yaw, "k_v", 0.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -461,9 +461,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } loop_count_++; - ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() + yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - + ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + pitch_k_v_ * pitch_vel_des + + ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); ctrl_yaw_.update(time, period); From 60da4b44372b13542476cee3f22165aeaa657860 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Thu, 21 Mar 2024 17:15:44 +0800 Subject: [PATCH 19/27] Modify for dynamic reconfig. --- rm_gimbal_controllers/src/gimbal_base.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index de470607..b1f8c2b3 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -528,11 +528,15 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.max_pid_yaw_pos_output = init_config.max_pid_yaw_pos_output; config.max_pid_pitch_pos_output = init_config.max_pid_pitch_pos_output; + config.yaw_k_v_ = init_config.yaw_k_v_; + config.pitch_k_v_ = init_config.pitch_k_v_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .max_pid_yaw_pos_output = config.max_pid_yaw_pos_output, .max_pid_pitch_pos_output = config.max_pid_pitch_pos_output, + .yaw_k_v_ = config.yaw_k_v_, + .pitch_k_v_ = config.pitch_k_v_, }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From 24963cd5d31c412d21981f76e3733e00302518e1 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 25 Mar 2024 18:32:34 +0800 Subject: [PATCH 20/27] Dynamic switch angle. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 ++ .../rm_gimbal_controllers/bullet_solver.h | 6 ++++- rm_gimbal_controllers/src/bullet_solver.cpp | 25 ++++++++++++++----- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 8f2b4f4a..628c46f3 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,5 +14,7 @@ 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("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) +gen.add("angle1", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0) +gen.add("angle2", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) 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 5ed6d081..2fa7f115 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,13 +46,14 @@ #include #include #include +#include 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, dt, timeout; + resistance_coff_qd_30, g, delay, dt, timeout, angle1, angle2; }; class BulletSolver @@ -85,6 +86,9 @@ class BulletSolver std::shared_ptr> path_real_pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; + rm_msgs::TrackData switch_target_; + ros::NodeHandle controller_nh_; + ros::Publisher switch_target_pub = controller_nh_.advertise("IsSwitchTarget", 100); Config config_{}; double max_track_target_vel_; bool dynamic_reconfig_initialized_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 94188c8a..76efd4ad 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -52,7 +52,9 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), - .timeout = getParam(controller_nh, "timeout", 0.) }; + .timeout = getParam(controller_nh, "timeout", 0.), + .angle1 = getParam(controller_nh, "angle1", 15.), + .angle2 = getParam(controller_nh, "angle2", 15.) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -116,17 +118,24 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double r = r1; double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = track_target_ ? - acos(r / target_rho) - M_PI / 12 + - (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : - M_PI / 12; + config_.angle1 = config_.angle1 * M_PI / 180; + config_.angle2 = config_.angle2 * M_PI / 180; + double switch_armor_angle = track_target_ ? acos(r / target_rho) - config_.angle1 + + (-acos(r / target_rho) + config_.angle1 + config_.angle2) * + std::abs(v_yaw) / max_track_target_vel_ : + config_.angle2; + bool switch_target = 0; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; + switch_target = 1; } + switch_target_.yaw = switch_armor_angle; + switch_target_.id = switch_target; + switch_target_pub.publish(switch_target_); int count{}; double error = 999; if (track_target_) @@ -316,6 +325,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.delay = init_config.delay; config.dt = init_config.dt; config.timeout = init_config.timeout; + config.angle1 = init_config.angle1; + config.angle2 = init_config.angle2; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -326,7 +337,9 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .g = config.g, .delay = config.delay, .dt = config.dt, - .timeout = config.timeout }; + .timeout = config.timeout, + .angle1 = config.angle1, + .angle2 = config.angle2 }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From c7fcc585b47de8e26fc344fc8c36ae5d06e2dfe5 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 31 Mar 2024 22:53:35 +0800 Subject: [PATCH 21/27] Modify method of calculate gimbal vel des. --- .../rm_gimbal_controllers/gimbal_base.h | 27 --------- rm_gimbal_controllers/src/gimbal_base.cpp | 60 ++++++++++++------- 2 files changed, 37 insertions(+), 50 deletions(-) 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 1f83a233..e26249be 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -54,7 +54,6 @@ #include #include #include -#include #include #include @@ -67,31 +66,6 @@ struct GimbalConfig double yaw_k_v_, pitch_k_v_; }; -class GimbalDesVel -{ -public: - GimbalDesVel(ros::NodeHandle& nh) - { - ros::NodeHandle nh_yaw = ros::NodeHandle(nh, "yaw"); - ros::NodeHandle nh_pitch = ros::NodeHandle(nh, "pitch"); - pitch_vel_des_lp_filter_ = std::make_shared(nh_pitch); - yaw_vel_des_lp_filter_ = std::make_shared(nh_yaw); - } - std::shared_ptr pitch_vel_des_lp_filter_, yaw_vel_des_lp_filter_; - void update(double yaw_vel_des, double pitch_vel_des, double period, const ros::Time& time) - { - if (period < 0) - return; - if (period > 0.1) - { - pitch_vel_des_lp_filter_->reset(); - yaw_vel_des_lp_filter_->reset(); - } - pitch_vel_des_lp_filter_->input(pitch_vel_des, time); - yaw_vel_des_lp_filter_->input(yaw_vel_des, time); - } -}; - class ChassisVel { public: @@ -186,7 +160,6 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; - std::shared_ptr gimbal_des_vel_; // ROS Interface ros::Time last_publish_time_{}; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index b1f8c2b3..0e3017d0 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -75,10 +75,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); - - ros::NodeHandle gimbal_des_vel_nh(controller_nh, "gimbal_des_vel"); - gimbal_des_vel_ = std::make_shared(gimbal_des_vel_nh); - ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); @@ -394,33 +390,51 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_yaw.z = ctrl_yaw_.joint_.getVelocity(); angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); } - - // compute gimbal vel des - double yaw_vel_des = 0., pitch_vel_des = 0.; double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - double tf_period = odom2gimbal_des_.header.stamp.toSec() - last_odom2gimbal_des_.header.stamp.toSec(); - double last_roll_des, last_pitch_des, last_yaw_des; - quatToRPY(last_odom2gimbal_des_.transform.rotation, last_roll_des, last_pitch_des, last_yaw_des); - double yaw_vel = angles::shortest_angular_distance(last_yaw_des, yaw_des) / tf_period; - double pitch_vel = angles::shortest_angular_distance(last_pitch_des, pitch_des) / tf_period; - gimbal_des_vel_->update(yaw_vel, pitch_vel, tf_period, time); - yaw_vel_des = gimbal_des_vel_->yaw_vel_des_lp_filter_->output(); - pitch_vel_des = gimbal_des_vel_->pitch_vel_des_lp_filter_->output(); - last_odom2gimbal_des_ = odom2gimbal_des_; - quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); pid_pitch_pos_.computeCommand(pitch_angle_error, period); pid_yaw_pos_.computeCommand(yaw_angle_error, period); - // limit feedforward output - // if (std::abs(pitch_vel_des) > config_.max_pid_pitch_pos_output) - // pitch_vel_des = pitch_vel_des > 0. ? config_.max_pid_pitch_pos_output : -config_.max_pid_pitch_pos_output; - // if (std::abs(pid_pitch_pos_.getCurrentCmd()) > config_.max_pid_yaw_pos_output) - // pid_pitch_pos_.setCurrentCmd(pid_pitch_pos_.getCurrentCmd() > 0. ? config_.max_pid_yaw_pos_output : - // -config_.max_pid_yaw_pos_output); + double yaw_vel_des = 0., pitch_vel_des = 0.; + if (state_ == RATE) + { + yaw_vel_des = cmd_gimbal_.rate_yaw; + pitch_vel_des = cmd_gimbal_.rate_pitch; + } + else if (state_ == TRACK) + { + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, data_track_.position, data_track_.velocity, + data_track_.yaw, data_track_.v_yaw, data_track_.radius_1, + data_track_.radius_2, data_track_.dz, data_track_.armors_num); + tf2::Vector3 target_pos_tf, target_vel_tf; + try + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + yaw_joint_urdf_->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); + + yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->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); + pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } + } // publish state if (loop_count_ % 10 == 0) From e32d5e2f535d9e9574338a32411b7d7be39bdab5 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 31 Mar 2024 22:55:25 +0800 Subject: [PATCH 22/27] Remove some unnecessary params. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 2 -- .../include/rm_gimbal_controllers/gimbal_base.h | 1 - rm_gimbal_controllers/src/gimbal_base.cpp | 9 +-------- 3 files changed, 1 insertion(+), 11 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 476392b5..0dd3e3e2 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -5,8 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("max_pid_yaw_pos_output", double_t, 0, "Pid yaw position max output", 0.0, 0, 99.0) -gen.add("max_pid_pitch_pos_output", double_t, 0, "Pid pitch position max output", 0.0, 0, 99.0) gen.add("yaw_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) gen.add("pitch_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) 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 e26249be..34c148c2 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -61,7 +61,6 @@ namespace rm_gimbal_controllers { struct GimbalConfig { - double max_pid_yaw_pos_output, max_pid_pitch_pos_output; // Input feedforward double yaw_k_v_, pitch_k_v_; }; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 0e3017d0..a5e13561 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -78,10 +78,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); - config_ = { .max_pid_yaw_pos_output = getParam(nh_pid_yaw_pos, "max_output", 0.), - .max_pid_pitch_pos_output = getParam(nh_pid_pitch_pos, "max_output", 0.), - .yaw_k_v_ = getParam(nh_pitch, "k_v", 0.), - .pitch_k_v_ = getParam(nh_yaw, "k_v", 0.) }; + config_ = { .yaw_k_v_ = getParam(nh_pitch, "k_v", 0.), .pitch_k_v_ = getParam(nh_yaw, "k_v", 0.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -540,15 +537,11 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin if (!dynamic_reconfig_initialized_) { GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml - config.max_pid_yaw_pos_output = init_config.max_pid_yaw_pos_output; - config.max_pid_pitch_pos_output = init_config.max_pid_pitch_pos_output; config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ - .max_pid_yaw_pos_output = config.max_pid_yaw_pos_output, - .max_pid_pitch_pos_output = config.max_pid_pitch_pos_output, .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, }; From 7e2eb4b82609be54db6c4d8f6d02baccb2dac018 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 31 Mar 2024 22:58:09 +0800 Subject: [PATCH 23/27] Remove some unnecessary header file. --- .../include/rm_gimbal_controllers/gimbal_base.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) 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 34c148c2..7001757e 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -41,21 +41,20 @@ #include #include #include -#include #include -#include +#include +#include #include #include #include -#include +#include #include #include #include -#include #include #include -#include #include +#include namespace rm_gimbal_controllers { From cd1799ddec518de60ec2df002262bf4c8f86efdf Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 13:14:31 +0800 Subject: [PATCH 24/27] Add angle limit for shooting. --- .../include/rm_gimbal_controllers/bullet_solver.h | 2 ++ rm_gimbal_controllers/src/bullet_solver.cpp | 8 ++++++++ 2 files changed, 10 insertions(+) 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 2fa7f115..fb81421b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -77,6 +77,7 @@ class BulletSolver 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); + bool isShootAfterDelay(); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -96,6 +97,7 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; int selected_armor_; bool track_target_; + bool is_shoot_after_delay_; geometry_msgs::Point target_pos_{}; double fly_time_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 76efd4ad..7d47c583 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -125,6 +125,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d std::abs(v_yaw) / max_track_target_vel_ : config_.angle2; bool switch_target = 0; + if ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) + is_shoot_after_delay_ = false; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { @@ -219,6 +222,11 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge } } +bool BulletSolver::isShootAfterDelay() +{ + return is_shoot_after_delay_; +} + void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) { marker_desire_.points.clear(); From c4eb15f70f78517112bae70c882c144daa86cfca Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 16:23:09 +0800 Subject: [PATCH 25/27] Add angle limit for shooting. --- .../include/rm_gimbal_controllers/bullet_solver.h | 6 ++++-- rm_gimbal_controllers/src/bullet_solver.cpp | 14 +++++++++++--- rm_gimbal_controllers/src/gimbal_base.cpp | 1 + 3 files changed, 16 insertions(+), 5 deletions(-) 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 fb81421b..4b89ddc8 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -47,6 +47,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -77,7 +78,7 @@ class BulletSolver 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); - bool isShootAfterDelay(); + void isShootAfterDelay(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -85,6 +86,7 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; + std::shared_ptr> pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; rm_msgs::TrackData switch_target_; @@ -97,7 +99,7 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; int selected_armor_; bool track_target_; - bool is_shoot_after_delay_; + double is_shoot_after_delay_ = 1.; geometry_msgs::Point target_pos_{}; double fly_time_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 7d47c583..7eb29b5e 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -82,6 +82,7 @@ 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)); + pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -127,7 +128,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bool switch_target = 0; if ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) - is_shoot_after_delay_ = false; + is_shoot_after_delay_ = 0.; + else + is_shoot_after_delay_ = 1.; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { @@ -222,9 +225,14 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge } } -bool BulletSolver::isShootAfterDelay() +void BulletSolver::isShootAfterDelay(const ros::Time& time) { - return is_shoot_after_delay_; + if (pub_->trylock()) + { + pub_->msg_.stamp = time; + pub_->msg_.error = is_shoot_after_delay_; + pub_->unlockAndPublish(); + } } void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index a5e13561..8c5d5da2 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -305,6 +305,7 @@ void Controller::track(const ros::Time& time) error_pub_->unlockAndPublish(); } bullet_solver_->bulletModelPub(odom2pitch_, time); + bullet_solver_->isShootAfterDelay(time); last_publish_time_ = time; } From cd1430f4b158c9c1035866873c0f20b6039707bd Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 18:43:14 +0800 Subject: [PATCH 26/27] Modify something unreasonable. --- rm_gimbal_controllers/src/bullet_solver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 7eb29b5e..142ab1a8 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -126,11 +126,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d std::abs(v_yaw) / max_track_target_vel_ : config_.angle2; bool switch_target = 0; - if ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) + is_shoot_after_delay_ = 1.; + if (((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && + track_target_) is_shoot_after_delay_ = 0.; - else - is_shoot_after_delay_ = 1.; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { From 3cdb8d3fd94ab621553d1b1e0ac7e96426d75d11 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 22:42:44 +0800 Subject: [PATCH 27/27] Delete something to dynamic reconfigure. --- .../include/rm_gimbal_controllers/bullet_solver.h | 6 +----- rm_gimbal_controllers/src/bullet_solver.cpp | 15 +++++---------- 2 files changed, 6 insertions(+), 15 deletions(-) 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 4b89ddc8..eef3783b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,7 +46,6 @@ #include #include #include -#include #include namespace rm_gimbal_controllers @@ -86,12 +85,9 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> pub_; + std::shared_ptr> is_shoot_after_delay_pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; - rm_msgs::TrackData switch_target_; - ros::NodeHandle controller_nh_; - ros::Publisher switch_target_pub = controller_nh_.advertise("IsSwitchTarget", 100); Config config_{}; double max_track_target_vel_; bool dynamic_reconfig_initialized_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 142ab1a8..e6764d6a 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -82,7 +82,7 @@ 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)); - pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); + is_shoot_after_delay_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -125,7 +125,6 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (-acos(r / target_rho) + config_.angle1 + config_.angle2) * std::abs(v_yaw) / max_track_target_vel_ : config_.angle2; - bool switch_target = 0; is_shoot_after_delay_ = 1.; if (((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && @@ -137,11 +136,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; - switch_target = 1; } - switch_target_.yaw = switch_armor_angle; - switch_target_.id = switch_target; - switch_target_pub.publish(switch_target_); int count{}; double error = 999; if (track_target_) @@ -227,11 +222,11 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge void BulletSolver::isShootAfterDelay(const ros::Time& time) { - if (pub_->trylock()) + if (is_shoot_after_delay_pub_->trylock()) { - pub_->msg_.stamp = time; - pub_->msg_.error = is_shoot_after_delay_; - pub_->unlockAndPublish(); + is_shoot_after_delay_pub_->msg_.stamp = time; + is_shoot_after_delay_pub_->msg_.error = is_shoot_after_delay_; + is_shoot_after_delay_pub_->unlockAndPublish(); } }