From 41621055dcca5fb72476ba0200a67410973706b6 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 6 Aug 2023 19:32:51 +0800 Subject: [PATCH 1/3] Avoid nan vel. --- rm_chassis_controllers/src/chassis_base.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index e6f0d197..2fcf50a6 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -281,10 +281,15 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p // integral vel to pos and angle tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); - odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); - odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec(); - odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec(); - double length = + double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); + if(length < 2000) + { + // avoid nan vel + odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); + odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec(); + odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec(); + } + length = std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + std::pow(angular_vel_odom.z, 2)); if (length > 0.001) { // avoid nan quat From d9c6fe59371a6a690052eab0be9bd3dbc8e5e43b Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 6 Aug 2023 20:01:38 +0800 Subject: [PATCH 2/3] Add max_odom_vel and delete useless params. --- .../include/rm_chassis_controllers/chassis_base.h | 3 ++- rm_chassis_controllers/src/chassis_base.cpp | 5 ++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 0f87e7d0..f12f499b 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -146,8 +146,9 @@ class ChassisBase : public controller_interface::MultiInterfaceController hardware_interface::EffortJointInterface* effort_joint_interface_{}; std::vector joint_handles_{}; - double wheel_base_{}, wheel_track_{}, wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, + double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{}, power_offset_{}; + double max_odom_vel_; bool enable_odom_tf_ = false; bool topic_update_ = false; bool publish_odom_tf_ = false; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 2fcf50a6..6ab68985 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -61,9 +61,8 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan return false; } wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02); - wheel_track_ = getParam(controller_nh, "wheel_track", 0.410); - wheel_base_ = getParam(controller_nh, "wheel_base", 0.320); twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6); + max_odom_vel_ = getParam(controller_nh,"max_odom_vel",0); enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true); publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false); @@ -282,7 +281,7 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); - if(length < 2000) + if(length < max_odom_vel_) { // avoid nan vel odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); From df4b9028c29f83c59f050c7615751088b02f6c42 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 6 Aug 2023 20:05:47 +0800 Subject: [PATCH 3/3] Run pre-commit. --- .../rm_chassis_controllers/chassis_base.h | 4 ++-- rm_chassis_controllers/src/chassis_base.cpp | 7 +++--- rm_gimbal_controllers/src/bullet_solver.cpp | 23 +++++++++++-------- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index f12f499b..d88be651 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -146,8 +146,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController hardware_interface::EffortJointInterface* effort_joint_interface_{}; std::vector joint_handles_{}; - double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, - velocity_coeff_{}, power_offset_{}; + double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{}, + power_offset_{}; double max_odom_vel_; bool enable_odom_tf_ = false; bool topic_update_ = false; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 6ab68985..bba37a5f 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -62,7 +62,7 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan } wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02); twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6); - max_odom_vel_ = getParam(controller_nh,"max_odom_vel",0); + max_odom_vel_ = getParam(controller_nh, "max_odom_vel", 0); enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true); publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false); @@ -280,8 +280,9 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p // integral vel to pos and angle tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); - double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); - if(length < max_odom_vel_) + double length = + std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); + if (length < max_odom_vel_) { // avoid nan vel odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e1e886ef..94188c8a 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -116,10 +116,10 @@ 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; + 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; 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.)) { @@ -287,13 +287,16 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec else { geometry_msgs::Point target_pos_after_fly_time_and_delay{}; - target_pos_after_fly_time_and_delay.x = pos.x + vel.x * (fly_time_ + delay) - - r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos_after_fly_time_and_delay.y = pos.y + vel.y * (fly_time_ + delay) - - r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.x = + pos.x + vel.x * (fly_time_ + delay) - + r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.y = + pos.y + vel.y * (fly_time_ + delay) - + r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); - error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x,2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y,2) + - std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z,2)); + error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); } return error; }