def send_depth_goal(self, depth): pos_old = NED(self.position.north, self.position.east, self.position.depth) goal = NED(pos_old.north, pos_old.east, depth) # send the goal to depth controller if action_library.send_depth_goal(self.stateRefPub, goal) == -1: return False # init error structure pos_err = [] # wait until goal is reached while not rospy.is_shutdown(): # distance from goal depth dl = abs(goal.depth - self.position.depth) # remembers the last 10 errors --> to change this, just replace 10 with desired number # (bigger err structure equals more precision, but longer time to acheive the goal) if len(pos_err) < 10: pos_err.append(dl) else: pos_err.pop(0) pos_err.append(dl) # goal precision in m --> for better precision replace the number with the smaller one, e.g. 0.001 for 1 mm precision # for worse precision, insert greater one, e.g. 1 for 1 m precision if (len(pos_err) == 10) and (fabs(sum(pos_err) / len(pos_err)) < 0.05): # mission is successfully finished return True else: # mission is still ongoing, sleep for 0.1 s rospy.sleep(rospy.Duration(0.1))
def send_depth_goal(self, depth): pos_old = NED(self.position.north, self.position.east, self.position.depth) goal = NED(pos_old.north, pos_old.east, depth) if action_library.send_depth_goal(self.stateRefPub, goal) == -1: return pos_err = [] while not rospy.is_shutdown(): dL = abs(goal.depth - pos_old.depth) dl = abs(goal.depth - self.position.depth) if len(pos_err) < 10: pos_err.append(dl) else: pos_err.pop(0) pos_err.append(dl) if (len(pos_err) == 10) and (fabs(sum(pos_err) / len(pos_err)) < 0.05): # mission is successfully finished return else: #mission is still ongoing rospy.sleep(0.1)