def battery_cb(self, msg):
self.batteryCall = NED(msg.north, msg.east, msg.depth)
#self.send_position_goal = self.batteryCall
pos_old = NED(self.position.north, self.position.east, self.position.depth)
goal = NED(msg.north, msg.east, pos_old.depth)
action_library.send_position_goal(self.stateRefPub, goal)
def battery_cb(self, msg):
self.batteryCall = NED(msg.north, msg.east, msg.depth)
#self.send_position_goal = self.batteryCall
pos_old = NED(self.position.north, self.position.east,
self.position.depth)
goal = NED(msg.north, msg.east, pos_old.depth)
action_library.send_position_goal(self.stateRefPub, goal)
def start_cb(self, msg):
battery = action_library.Battery()
print battery.get_level()
print "################"
return
self.startTime = rospy.get_time()
# enable and configure controllers
self.velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl)
self.velcon_enable(enable=True)
self.fadp_enable = rospy.ServiceProxy('FADP_enable', EnableControl)
self.fadp_enable(enable=True)
self.config_vel_controller = rospy.ServiceProxy(
'ConfigureVelocityController', ConfigureVelocityController)
self.config_vel_controller(ControllerName="FADP",
desired_mode=[2, 2, 2, 0, 0, 0])
# submerge
while self.position is None:
rospy.sleep(0.1)
action_library.send_position_goal(
self.stateRefPub, NED(self.position.north, self.position.east, 5))
depthOld = self.position.depth
posErr = []
while True:
dL = abs(self.position.depth - depthOld)
dl = abs(5 - self.position.depth)
if len(posErr) < 20:
posErr.append(dl)
else:
posErr.pop(0)
posErr.append(dl)
if (len(posErr) == 20) and (fabs(sum(posErr) / len(posErr)) < 0.1
): # mission is successfully finished
break
self.start = True
self.startPub.publish(msg)
def start_cb(self, msg):
battery = action_library.Battery()
print battery.get_level()
print "################"
return
self.startTime = rospy.get_time()
# enable and configure controllers
self.velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl)
self.velcon_enable(enable=True)
self.fadp_enable = rospy.ServiceProxy('FADP_enable', EnableControl)
self.fadp_enable(enable=True)
self.config_vel_controller = rospy.ServiceProxy('ConfigureVelocityController', ConfigureVelocityController)
self.config_vel_controller(ControllerName="FADP", desired_mode=[2, 2, 2, 0, 0, 0])
# submerge
while self.position is None:
rospy.sleep(0.1)
action_library.send_position_goal(self.stateRefPub, NED(self.position.north, self.position.east, 5))
depthOld = self.position.depth
posErr = []
while True:
dL = abs(self.position.depth - depthOld)
dl = abs(5 - self.position.depth)
if len(posErr) < 20:
posErr.append(dl)
else:
posErr.pop(0)
posErr.append(dl)
if (len(posErr) == 20) and (fabs(sum(posErr) / len(posErr)) < 0.1): # mission is successfully finished
break
self.start = True
self.startPub.publish(msg)
