From f6cd322e0acda3d3d6d96152f85dc1f376a9b949 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 20 Dec 2023 19:20:31 +0900 Subject: [PATCH 1/6] [Beetle] 12/20, yaw control is not working well --- robots/beetle/config/BeetleControl_sim.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robots/beetle/config/BeetleControl_sim.yaml b/robots/beetle/config/BeetleControl_sim.yaml index 251b291e8..16ab47b64 100644 --- a/robots/beetle/config/BeetleControl_sim.yaml +++ b/robots/beetle/config/BeetleControl_sim.yaml @@ -3,7 +3,7 @@ aerial_robot_control_name: aerial_robot_control/beetle_controller controller: torque_allocation_matrix_inv_pub_interval: 0.05 wrench_allocation_matrix_pub_interval: 0.1 - gimbal_calc_in_fc : true + gimbal_calc_in_fc : false i_term_rp_calc_in_pc: true wrench_estimate_update_rate: 100 momentum_observer_force_weight: 3 From 6de11e1b79b21f048aeab2a326df8e1fc852fce5 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 20 Dec 2023 19:22:29 +0900 Subject: [PATCH 2/6] [Beetle] 12/20, yaw control is not working well --- robots/beetle/launch/bringup.launch | 2 -- 1 file changed, 2 deletions(-) diff --git a/robots/beetle/launch/bringup.launch b/robots/beetle/launch/bringup.launch index 51a7e9b21..545b5ef3e 100644 --- a/robots/beetle/launch/bringup.launch +++ b/robots/beetle/launch/bringup.launch @@ -1,5 +1,3 @@ - - ########### launch config ########### From 7fa173b36e03464ddeb0a1f1520e3f916302a437 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 20 Dec 2023 19:25:44 +0900 Subject: [PATCH 3/6] [Beetle] 12/20, yaw control is not working well --- .../launch/test/apriltag_detection.launch | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/robots/beetle/launch/test/apriltag_detection.launch b/robots/beetle/launch/test/apriltag_detection.launch index 506b8164d..b70806aa2 100644 --- a/robots/beetle/launch/test/apriltag_detection.launch +++ b/robots/beetle/launch/test/apriltag_detection.launch @@ -1,21 +1,4 @@ - - - - - - - - - - - - - - - - - - + From bb1bb5fe76fe82d6dcf5a84f82ca10982509484a Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 20 Dec 2023 19:27:16 +0900 Subject: [PATCH 4/6] [Beetle] control file for real machine --- robots/beetle/script/apriltagposition.py | 94 +++++++++++------------- 1 file changed, 43 insertions(+), 51 deletions(-) diff --git a/robots/beetle/script/apriltagposition.py b/robots/beetle/script/apriltagposition.py index 2d22c53f4..6d962001a 100755 --- a/robots/beetle/script/apriltagposition.py +++ b/robots/beetle/script/apriltagposition.py @@ -13,9 +13,9 @@ def __init__(self): # parameter's for PID control #position gains - self.kp = -1.0 - self.ki = -0.001 - self.kd = -0.05 + self.kp = -0.2 + self.ki = -0.0002 + self.kd = -0.01 #duration self.dt = 0.025 @@ -47,7 +47,7 @@ def __init__(self): self.lastEuler = np.array([0.0, 0.0, 0.0]) #distance - self.targetDistance = np.array([0.08171, -0.01239, 0.01017]) #offset + self.targetDistance = np.array([1.08171, -0.01239, 0.01017]) #x offset = 1.58171 self.currentDistance = np.array([0.0, 0.0, 0.0]) self.lastDistance = np.array([0.0, 0.0, 0.0]) self.currentEucDistance = 0 @@ -57,14 +57,14 @@ def __init__(self): self.sub = rospy.Subscriber('/beetle1/tag_detections', AprilTagDetectionArray, self.callback1) self.yaw_sub = rospy.Subscriber('/beetle1/mocap/pose', PoseStamped, self.callback2) - #boolean for docking - self.docking_msg = Bool() - self.docking_pub = rospy.Publisher('/beetle1/docking_cmd', Bool, queue_size=1) + # #boolean for docking + # self.docking_msg = Bool() + # self.docking_pub = rospy.Publisher('/beetle1/docking_cmd', Bool, queue_size=1) - #flags for docking - self.flag_msg = KeyValue() - self.flag_pub_1 = rospy.Publisher('/beetle1/assembly_flag', KeyValue, queue_size=1) - self.flag_pub_2 = rospy.Publisher('/beetle2/assembly_flag', KeyValue, queue_size=1) + # #flags for docking + # self.flag_msg = KeyValue() + # self.flag_pub_1 = rospy.Publisher('/beetle1/assembly_flag', KeyValue, queue_size=1) + # self.flag_pub_2 = rospy.Publisher('/beetle2/assembly_flag', KeyValue, queue_size=1) #others self.rate = rospy.Rate(40) @@ -82,28 +82,28 @@ def calculateError(self): self.err_i += self.err self.pre_err = self.err - def dockingVerifier(self): - #set docking message - if self.currentEucDistance <= 0.01: - if self.currentEuler[1] <=2: - self.flag_msg.key = '1' - self.flag_msg.value = '1' - else: - self.flag_msg.key = '0' - self.flag_msg.value = '0' + # def dockingVerifier(self): + # #set docking message + # if self.currentEucDistance <= 0.01: + # if self.currentEuler[1] <=2: + # self.flag_msg.key = '1' + # self.flag_msg.value = '1' + # else: + # self.flag_msg.key = '0' + # self.flag_msg.value = '0' def defineYaw(self): - self.yaw = self.currentmocapEuler[2] + self.currentEuler[1] - if self.yaw >= 2: - self.nav_msg.yaw_nav_mode = 2 - else: - self.nav_msg.yaw_nav_mode = 0 + self.yaw = self.currentmocapEuler[2] - self.currentEuler[2] + # if self.yaw <= -0.03: + # self.nav_msg.yaw_nav_mode = 2 + # else: + # self.nav_msg.yaw_nav_mode = 0 def callback1(self,data): if data.detections: #set values to current pose position = data.detections[0].pose.pose.pose.position - self.currentDistance = [position.z, -position.x, -position.y] #x = z, y = -x, z = -y + self.currentDistance = [position.z, -position.x, -position.y] #calculate current euclid distance self.currentEucDistance = ((position.z-self.targetDistance[0])**2 + (-position.x-self.targetDistance[1])**2 + (-position.y-self.targetDistance[2])**2)**0.5 @@ -114,9 +114,9 @@ def callback1(self,data): #calculate current Euler angle orientation = data.detections[0].pose.pose.pose.orientation self.currentQuarter = [orientation.x, orientation.y, orientation.z, orientation.w] - self.currentEuler = np.degrees(np.array([np.arctan2(2 * (orientation.w * orientation.x + orientation.y * orientation.z), 1 - 2 * (orientation.x**2 + orientation.y**2)), - np.arcsin(2 * (orientation.w * orientation.y - orientation.z * orientation.x)), - np.arctan2(2 * (orientation.w * orientation.z + orientation.x * orientation.y), 1 - 2 * (orientation.y**2 + orientation.z**2))])) + self.currentEuler = np.array([np.arctan2(2 * (orientation.x * orientation.y + orientation.w * orientation.z), 1 - 2 * (orientation.x**2 + orientation.y**2)), + np.arcsin(2 * (orientation.w * orientation.y - orientation.x * orientation.z)), + np.arctan2(2 * (orientation.w * orientation.z + orientation.x * orientation.y), 1 - 2 * (orientation.y**2 + orientation.z**2))]) #set angular log self.lastEuler = self.currentEuler @@ -146,7 +146,7 @@ def callback2(self, data): self.currentmocapEuler = self.lastmocapEuler def main(self): - self.docking_msg.data = True + # self.docking_msg.data = True while not rospy.is_shutdown(): #callback1, 2 @@ -181,29 +181,21 @@ def main(self): self.nav_msg.target_vel_z = self.vel[2] self.pub.publish(self.nav_msg) - #beetle1 mode verifier - self.dockingVerifier() + # #beetle1 mode verifier + # self.dockingVerifier() - #switch beetle1 into assembled mode - # true ->activated false->disactivate - self.docking_pub.publish(self.docking_msg) + # #switch beetle1 into assembled mode + # # true ->activated false->disactivate + # self.docking_pub.publish(self.docking_msg) - #flag_msg = KeyValue() - #flag_msg.key = '1' - #flag_msg.value = '1' # 1 -> assembly, 2->separate - self.flag_pub_1.publish(self.flag_msg) - self.flag_pub_2.publish(self.flag_msg) - - #rospy.loginfo(self.currentDistance) - rospy.loginfo(self.currentEucDistance) - #rospy.loginfo(self.vel) - #rospy.loginfo(self.currentQuarter) - #rospy.loginfo(self.currentmocapEuler) - rospy.loginfo(self.currentEuler) - #rospy.loginfo(self.yaw) - rospy.loginfo(self.flag_msg) - #rospy.loginfo(self.tag_lost_flag) - rospy.loginfo("") + # #flag_msg = KeyValue() + # #flag_msg.key = '1' + # #flag_msg.value = '1' # 1 -> assembly, 2->separate + # self.flag_pub_1.publish(self.flag_msg) + # self.flag_pub_2.publish(self.flag_msg) + + rospy.loginfo(self.currentDistance) + self.rate.sleep() if __name__ == "__main__": From dbaab08898abd0b34bc42dd2ee41a18fe68ee4a2 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 20 Dec 2023 19:28:08 +0900 Subject: [PATCH 5/6] [Beetle] control file for simulation, yaw control is not working well --- .../script/apriltagposition_simulation.py | 230 ++++++++++++++++++ 1 file changed, 230 insertions(+) create mode 100755 robots/beetle/script/apriltagposition_simulation.py diff --git a/robots/beetle/script/apriltagposition_simulation.py b/robots/beetle/script/apriltagposition_simulation.py new file mode 100755 index 000000000..e9240995d --- /dev/null +++ b/robots/beetle/script/apriltagposition_simulation.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python +import rospy +from apriltag_ros.msg import AprilTagDetectionArray +from std_msgs.msg import Float32, Float64, Bool +from geometry_msgs.msg import Twist, PoseStamped +from aerial_robot_msgs.msg import FlightNav +import numpy as np +import tf.transformations as tf +from diagnostic_msgs.msg import KeyValue + +class aprilPIDcontroller(): + def __init__(self): + rospy.init_node("aprilpidVel") + # parameter's for PID control + + #position gains + self.kp = -2.0 #-0.2 + self.ki = -0.005 #-0.0002 + self.kd = -0.05 #-0.01 + + #duration + self.dt = 0.025 + + #position terms + self.p_term = np.array([0.0, 0.0, 0.0]) + self.i_term = np.array([0.0, 0.0, 0.0]) + self.d_term = np.array([0.0, 0.0, 0.0]) + + #term corrector + self.TermCorrector = np.array([0.5, 1, 1]) + + #position error + self.err_i = np.array([0.0, 0.0, 0.0]) + self.err = np.array([0.0, 0.0, 0.0]) + self.pre_err = np.array([0.0, 0.0, 0.0]) + + #vel, yaw + self.vel = np.array([0.0, 0.0, 0.0]) + self.yaw = 0 + self.yaw_pre = 0 + + #angular + self.currentmocapQuarter = np.array([0.0, 0.0, 0.0, 0.0]) + self.currentmocapEuler = np.array([0.0, 0.0, 0.0]) + self.lastmocapEuler = np.array([0.0, 0.0, 0.0]) + self.currentQuarter = np.array([0.0, 0.0, 0.0, 0.0]) + self.currentEuler = np.array([0.0, 0.0, 0.0]) + self.lastEuler = np.array([0.0, 0.0, 0.0]) + + #distance + self.targetDistance = np.array([0.58171, -0.01239, 0.01017]) #x offset = 1.58171 + self.currentDistance = np.array([0.0, 0.0, 0.0]) + self.lastDistance = np.array([0.0, 0.0, 0.0]) + self.currentEucDistance = 0 + + #ros + self.pub = rospy.Publisher('/beetle1/uav/nav', FlightNav, queue_size=1) + self.sub = rospy.Subscriber('/beetle1/tag_detections', AprilTagDetectionArray, self.callback1) + self.yaw_sub = rospy.Subscriber('/beetle1/mocap/pose', PoseStamped, self.callback2) + + # #boolean for docking + # self.docking_msg = Bool() + # self.docking_pub = rospy.Publisher('/beetle1/docking_cmd', Bool, queue_size=1) + + # #flags for docking + # self.flag_msg = KeyValue() + # self.flag_pub_1 = rospy.Publisher('/beetle1/assembly_flag', KeyValue, queue_size=1) + # self.flag_pub_2 = rospy.Publisher('/beetle2/assembly_flag', KeyValue, queue_size=1) + + #others + self.rate = rospy.Rate(40) + self.nav_msg = FlightNav() + + #apriltag lost flag + self.tag_lost_flag = False + + #mocap lost flag + self.mocap_lost_flag = False + + self.pre_yaw_diff = 0 + self.yaw_set_flag = False + + def calculateError(self): + #calculate errors for PID + self.err = self.targetDistance - self.currentDistance + self.err_i += self.err + self.pre_err = self.err + + # def dockingVerifier(self): + # #set docking message + # if self.currentEucDistance <= 0.01: + # if self.currentEuler[1] <=2: + # self.flag_msg.key = '1' + # self.flag_msg.value = '1' + # else: + # self.flag_msg.key = '0' + # self.flag_msg.value = '0' + + def defineYaw(self): + if not self.yaw_set_flag: + self.pre_yaw_diff = self.currentEuler[1] + self.yaw_set_flag = True + rospy.loginfo("set") + yaw_chage_diff = abs(self.currentEuler[1] - self.pre_yaw_diff) + if yaw_chage_diff > 0.05: + rospy.loginfo("ignore unrelable data") + return + self.pre_yaw_diff = self.currentEuler[1] + self.yaw = self.currentmocapEuler[2] - self.currentEuler[1] + rospy.loginfo(self.yaw) + if self.currentEuler[1] <= -0.1 or self.currentEuler[1] >= 0.1: + self.nav_msg.yaw_nav_mode = 2 + self.nav_msg.target_yaw = self.yaw + else: + self.nav_msg.yaw_nav_mode = 0 + + def callback1(self,data): + if data.detections: + #set values to current pose + position = data.detections[0].pose.pose.pose.position + self.currentDistance = [position.z, -position.x, -position.y] + + #calculate current euclid distance + self.currentEucDistance = ((position.z-self.targetDistance[0])**2 + (-position.x-self.targetDistance[1])**2 + (-position.y-self.targetDistance[2])**2)**0.5 + + #set distance log + self.lastDistance = self.currentDistance + + #calculate current Euler angle + orientation = data.detections[0].pose.pose.pose.orientation + self.currentQuarter = [orientation.x, orientation.y, orientation.z, orientation.w] + # self.currentEuler = np.array([np.arctan2(2 * (orientation.x * orientation.y + orientation.w * orientation.z), 1 - 2 * (orientation.x**2 + orientation.y**2)), + # np.arcsin(2 * (orientation.w * orientation.y - orientation.x * orientation.z)), + # np.arctan2(2 * (orientation.w * orientation.z + orientation.x * orientation.y), 1 - 2 * (orientation.y**2 + orientation.z**2))]) + self.currentEuler = tf.euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w)) + + #set angular log + self.lastEuler = self.currentEuler + + self.tag_lost_flag = False + + else: + self.tag_lost_flag = True + + #load log + self.currentDistance = self.lastDistance + self.currentEuler = self.lastEuler + + def callback2(self, data): + if data.pose: + moc = data.pose.orientation + self.currentmocapQuarter = [moc.x, moc.y, moc.z, moc.w] + #calculate current mocap Euler angular + self.currentmocapEuler = np.radians(np.array([np.arctan2(2 * (moc.w * moc.x + moc.y * moc.z), 1 - 2 * (moc.x**2 + moc.y**2)), + np.arcsin(2 * (moc.w * moc.y - moc.z * moc.x)), + np.arctan2(2 * (moc.w * moc.z + moc.x * moc.y), 1 - 2 * (moc.y**2 + moc.z**2))])) + self.lastmocapEuler = self.currentmocapEuler + self.mocap_lost_flag = False + + else: + self.mocap_lost_flag = True + self.currentmocapEuler = self.lastmocapEuler + + def main(self): + # self.docking_msg.data = True + while not rospy.is_shutdown(): + #callback1, 2 + + #calcurate error + self.calculateError() + + #calculate term + self.p_term = self.kp * self.err + self.i_term = self.ki * self.err_i + self.d_term = self.kd * (self.err - self.pre_err) / self.dt + + # calcurate terms + if self.currentEucDistance <= 0.3: + #weaken x PID + self.p_term *= self.TermCorrector + self.i_term *= self.TermCorrector + self.d_term *= self.TermCorrector + + # calculate main process + self.vel = self.p_term + self.i_term + self.d_term + + #publish message + self.nav_msg.control_frame = 1 + self.nav_msg.target = 1 + self.nav_msg.pos_xy_nav_mode = 1 + + self.nav_msg.target_vel_x = self.vel[0] + self.nav_msg.target_vel_y = self.vel[1] + + self.defineYaw() + self.nav_msg.pos_z_nav_mode = 1 + self.nav_msg.target_vel_z = self.vel[2] + self.pub.publish(self.nav_msg) + + # #beetle1 mode verifier + # self.dockingVerifier() + + # #switch beetle1 into assembled mode + # # true ->activated false->disactivate + # self.docking_pub.publish(self.docking_msg) + + # #flag_msg = KeyValue() + # #flag_msg.key = '1' + # #flag_msg.value = '1' # 1 -> assembly, 2->separate + # self.flag_pub_1.publish(self.flag_msg) + # self.flag_pub_2.publish(self.flag_msg) + + # rospy.loginfo(self.vel) + #rospy.loginfo(self.currentmocapEuler[2]) + #rospy.loginfo(self.currenotEuler[1]) + # rospy.loginfo(self.targetDistance[0]) + # rospy.loginfo(self.currentDistance[0]) + # rospy.loginfo(self.err[0]) + # rospy.loginfo(self.p_term) + # rospy.loginfo(self.i_term) + #rospy.loginfo(self.yaw) + rospy.loginfo("") + + self.rate.sleep() + +if __name__ == "__main__": + try: + aprilpid = aprilPIDcontroller() + aprilpid.main() + except rospy.ROSInterruptException: pass From b5c17e3ec77129d40c5da222d93a569c5bddce18 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 27 Dec 2023 21:33:36 +0900 Subject: [PATCH 6/6] [Beetle] succeded to recover yaw angle --- .../script/apriltagposition_simulation.py | 175 +++++++++++++----- 1 file changed, 124 insertions(+), 51 deletions(-) diff --git a/robots/beetle/script/apriltagposition_simulation.py b/robots/beetle/script/apriltagposition_simulation.py index e9240995d..33e011fbc 100755 --- a/robots/beetle/script/apriltagposition_simulation.py +++ b/robots/beetle/script/apriltagposition_simulation.py @@ -3,6 +3,7 @@ from apriltag_ros.msg import AprilTagDetectionArray from std_msgs.msg import Float32, Float64, Bool from geometry_msgs.msg import Twist, PoseStamped +from nav_msgs.msg import Odometry from aerial_robot_msgs.msg import FlightNav import numpy as np import tf.transformations as tf @@ -11,35 +12,50 @@ class aprilPIDcontroller(): def __init__(self): rospy.init_node("aprilpidVel") - # parameter's for PID control + # parameters for PID control - #position gains + #gains self.kp = -2.0 #-0.2 self.ki = -0.005 #-0.0002 self.kd = -0.05 #-0.01 + self.recovery_gain = 3 + self.yaw_kp = -0.4 + self.yaw_ki = -0.001 + self.yaw_kd = -0.02 #duration self.dt = 0.025 - #position terms + #terms self.p_term = np.array([0.0, 0.0, 0.0]) self.i_term = np.array([0.0, 0.0, 0.0]) self.d_term = np.array([0.0, 0.0, 0.0]) + self.yaw_p_term = 0.0 + self.yaw_i_term = 0.0 + self.yaw_d_term = 0.0 #term corrector self.TermCorrector = np.array([0.5, 1, 1]) - #position error + #position and angular error self.err_i = np.array([0.0, 0.0, 0.0]) self.err = np.array([0.0, 0.0, 0.0]) self.pre_err = np.array([0.0, 0.0, 0.0]) + self.yaw_err_i = 0.0 + self.yaw_err = 0.0 + self.yaw_pre_err = 0.0 #vel, yaw self.vel = np.array([0.0, 0.0, 0.0]) - self.yaw = 0 - self.yaw_pre = 0 + self.vel_recovery = np.array([0.0, 0.0, 0.0]) + self.currentVelocity = np.array([0.0, 0.0, 0.0]) + self.velocitylog_queue = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) + self.yaw = 0.0 + self.yaw_pre = 0.0 + self.target_yaw = 0.0 + self.pre_yaw_diff = 0.0 - #angular + #angular self.currentmocapQuarter = np.array([0.0, 0.0, 0.0, 0.0]) self.currentmocapEuler = np.array([0.0, 0.0, 0.0]) self.lastmocapEuler = np.array([0.0, 0.0, 0.0]) @@ -48,14 +64,16 @@ def __init__(self): self.lastEuler = np.array([0.0, 0.0, 0.0]) #distance - self.targetDistance = np.array([0.58171, -0.01239, 0.01017]) #x offset = 1.58171 + self.targetDistance = np.array([0.38171, 0.009239, 0.01017]) #x offset = 1.58171 self.currentDistance = np.array([0.0, 0.0, 0.0]) self.lastDistance = np.array([0.0, 0.0, 0.0]) - self.currentEucDistance = 0 + self.distancelog_queue = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) + self.currentEucDistance = 0.0 #ros self.pub = rospy.Publisher('/beetle1/uav/nav', FlightNav, queue_size=1) self.sub = rospy.Subscriber('/beetle1/tag_detections', AprilTagDetectionArray, self.callback1) + self.sub = rospy.Subscriber('/beetle1/uav/cog/odom', Odometry, self.callback3) self.yaw_sub = rospy.Subscriber('/beetle1/mocap/pose', PoseStamped, self.callback2) # #boolean for docking @@ -71,20 +89,20 @@ def __init__(self): self.rate = rospy.Rate(40) self.nav_msg = FlightNav() - #apriltag lost flag + #flags self.tag_lost_flag = False - - #mocap lost flag self.mocap_lost_flag = False - self.pre_yaw_diff = 0 - self.yaw_set_flag = False - def calculateError(self): - #calculate errors for PID + #calculate errors for position and yaw control + self.pre_err = self.err self.err = self.targetDistance - self.currentDistance self.err_i += self.err - self.pre_err = self.err + + self.yaw_err = self.yaw - self.currentmocapEuler[2] + self.yaw_err_i += self.yaw_err + self.yaw_pre_err = self.yaw_err + # def dockingVerifier(self): # #set docking message @@ -97,70 +115,110 @@ def calculateError(self): # self.flag_msg.value = '0' def defineYaw(self): - if not self.yaw_set_flag: - self.pre_yaw_diff = self.currentEuler[1] - self.yaw_set_flag = True - rospy.loginfo("set") - yaw_chage_diff = abs(self.currentEuler[1] - self.pre_yaw_diff) - if yaw_chage_diff > 0.05: - rospy.loginfo("ignore unrelable data") - return - self.pre_yaw_diff = self.currentEuler[1] - self.yaw = self.currentmocapEuler[2] - self.currentEuler[1] - rospy.loginfo(self.yaw) - if self.currentEuler[1] <= -0.1 or self.currentEuler[1] >= 0.1: - self.nav_msg.yaw_nav_mode = 2 - self.nav_msg.target_yaw = self.yaw - else: - self.nav_msg.yaw_nav_mode = 0 + self.nav_msg.yaw_nav_mode = 2 + self.nav_msg.target_yaw = self.yaw + # if abs(self.currentEuler[1]) >= 0.1: + # self.nav_msg.yaw_nav_mode = 2 + # self.nav_msg.target_yaw = self.target_yaw + + # if self.currentEuler[1] >= 0.1: + # self.nav_msg.target_yaw = self.yaw - 0.05 + # rospy.loginfo("+") + # elif self.currentEuler[1] <= 0.1: + # self.nav_msg.target_yaw = self.yaw + 0.05 + # rospy.loginfo("-") + # else: + # self.nav_msg.target_yaw = self.yaw + # else: + # self.nav_msg.yaw_nav_mode = 0 + def callback1(self,data): if data.detections: #set values to current pose position = data.detections[0].pose.pose.pose.position - self.currentDistance = [position.z, -position.x, -position.y] + self.currentDistance = np.array([position.z, -position.x, -position.y]) + currentDistance_log = np.array([[position.z, -position.x, -position.y]]) #calculate current euclid distance self.currentEucDistance = ((position.z-self.targetDistance[0])**2 + (-position.x-self.targetDistance[1])**2 + (-position.y-self.targetDistance[2])**2)**0.5 #set distance log self.lastDistance = self.currentDistance + self.distancelog_queue = np.append(self.distancelog_queue, currentDistance_log, axis = 0) + if len(self.distancelog_queue) > 10: + self.distancelog_queue = np.delete(self.distancelog_queue, 0, axis = 0) + #rospy.loginfo(self.distancelog_queue) + # self.double_lastDistance = self.lastDistance + # self.lastDistance = self.currentDistance - #calculate current Euler angle + #set values to current quarter angle orientation = data.detections[0].pose.pose.pose.orientation self.currentQuarter = [orientation.x, orientation.y, orientation.z, orientation.w] - # self.currentEuler = np.array([np.arctan2(2 * (orientation.x * orientation.y + orientation.w * orientation.z), 1 - 2 * (orientation.x**2 + orientation.y**2)), - # np.arcsin(2 * (orientation.w * orientation.y - orientation.x * orientation.z)), - # np.arctan2(2 * (orientation.w * orientation.z + orientation.x * orientation.y), 1 - 2 * (orientation.y**2 + orientation.z**2))]) + + #calculate current Euler angle self.currentEuler = tf.euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w)) #set angular log self.lastEuler = self.currentEuler + + #calculate target yaw + yaw_change_diff = abs(self.currentEuler[1] - self.pre_yaw_diff) + if yaw_change_diff > 0.2: + rospy.loginfo("ignore unreliable data!") + self.pre_yaw_diff = self.currentEuler[1] + return + # rospy.loginfo("stable!") + self.pre_yaw_diff = self.currentEuler[1] + self.yaw = self.currentmocapEuler[2] + self.currentEuler[1] + # rospy.loginfo(self.currentmocapEuler[2]) + # rospy.loginfo(self.currentEuler[1]) + # rospy.loginfo(self.yaw) self.tag_lost_flag = False - + else: self.tag_lost_flag = True #load log - self.currentDistance = self.lastDistance + average_position = np.mean(self.distancelog_queue, axis = 0) + self.currentDistance = self.lastDistance + self.recovery_gain * (self.lastDistance - average_position) self.currentEuler = self.lastEuler + #calculate target yaw + yaw_change_diff = abs(self.currentEuler[1] - self.pre_yaw_diff) + if yaw_change_diff > 0.2: + rospy.loginfo("ignore unreliable data!") + self.pre_yaw_diff = self.currentEuler[1] + return + self.pre_yaw_diff = self.currentEuler[1] + self.yaw = self.currentmocapEuler[2] + self.currentEuler[1] + def callback2(self, data): if data.pose: moc = data.pose.orientation self.currentmocapQuarter = [moc.x, moc.y, moc.z, moc.w] #calculate current mocap Euler angular - self.currentmocapEuler = np.radians(np.array([np.arctan2(2 * (moc.w * moc.x + moc.y * moc.z), 1 - 2 * (moc.x**2 + moc.y**2)), - np.arcsin(2 * (moc.w * moc.y - moc.z * moc.x)), - np.arctan2(2 * (moc.w * moc.z + moc.x * moc.y), 1 - 2 * (moc.y**2 + moc.z**2))])) + self.currentmocapEuler = tf.euler_from_quaternion((moc.x, moc.y, moc.z, moc.w)) self.lastmocapEuler = self.currentmocapEuler self.mocap_lost_flag = False else: self.mocap_lost_flag = True self.currentmocapEuler = self.lastmocapEuler - + + def callback3(self, data): + #set values to current velocity + odom = data.twist.twist.linear + self.currentVelocity = np.array([[odom.x, odom.y, odom.z]]) + # rospy.loginfo(self.currentVelocity) + if not self.tag_lost_flag: + self.velocitylog_queue = np.append(self.velocitylog_queue, self.currentVelocity, axis = 0) + if len(self.velocitylog_queue) > 10: + self.velocitylog_queue = np.delete(self.velocitylog_queue, 0, axis = 0) + average_velocity = np.mean(self.velocitylog_queue, axis = 0) + self.vel_recovery = -1.0 * average_velocity + def main(self): # self.docking_msg.data = True while not rospy.is_shutdown(): @@ -173,16 +231,29 @@ def main(self): self.p_term = self.kp * self.err self.i_term = self.ki * self.err_i self.d_term = self.kd * (self.err - self.pre_err) / self.dt + + self.yaw_p_term = -self.yaw_kp * self.yaw_err + self.yaw_i_term = -self.yaw_ki * self.yaw_err_i + self.yaw_d_term = -self.yaw_kd * (self.yaw_err - self.yaw_pre_err) / self.dt + # rospy.loginfo(self.yaw_p_term) # calcurate terms - if self.currentEucDistance <= 0.3: + if self.currentEucDistance <= 0.5: #weaken x PID self.p_term *= self.TermCorrector self.i_term *= self.TermCorrector self.d_term *= self.TermCorrector # calculate main process - self.vel = self.p_term + self.i_term + self.d_term + if not self.tag_lost_flag: + self.vel = self.p_term + self.i_term + self.d_term + self.target_yaw = self.yaw_p_term + self.yaw_i_term + self.yaw_d_term + else: + self.err = 0 + self.err_i = 0 + self.pre_err = 0 + self.yaw_nav_mode = 0 + self.vel = self.vel_recovery #publish message self.nav_msg.control_frame = 1 @@ -210,15 +281,17 @@ def main(self): # self.flag_pub_1.publish(self.flag_msg) # self.flag_pub_2.publish(self.flag_msg) - # rospy.loginfo(self.vel) - #rospy.loginfo(self.currentmocapEuler[2]) - #rospy.loginfo(self.currenotEuler[1]) + #rospy.loginfo(self.yaw) + # rospy.loginfo(self.currentmocapEuler[2]) + # rospy.loginfo(self.currenotEuler[1]) # rospy.loginfo(self.targetDistance[0]) - # rospy.loginfo(self.currentDistance[0]) + # rospy.loginfo(self.currentDistance[2]) # rospy.loginfo(self.err[0]) # rospy.loginfo(self.p_term) # rospy.loginfo(self.i_term) - #rospy.loginfo(self.yaw) + #rospy.loginfo(self.target_yaw) + rospy.loginfo(self.yaw_err) + rospy.loginfo(self.target_yaw) rospy.loginfo("") self.rate.sleep()