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)
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)
