def takeOff(self):
goal = Goal()
goal.p.x = self.pose.position.x
goal.p.y = self.pose.position.y
goal.p.z = self.pose.position.z
goal.psi = quat2yaw(self.pose.orientation)
goal.power = True
#Turn on the motors
alt_taken_off = self.pose.position.z + 1.0
#Altitude when hovering after taking off
#Note that self.pose.position is being updated in the parallel callback
######## Commented for simulations
while (abs(self.pose.position.z - alt_taken_off) > 0.2):
goal.p.z = min(goal.p.z + 0.0035, alt_taken_off)
rospy.sleep(0.004)
rospy.loginfo_throttle(
0.5, "Taking off..., error={}".format(self.pose.position.z -
alt_taken_off))
self.sendGoal(goal)
########
rospy.sleep(0.1)
self.whoplans.value = self.whoplans.PANTHER
self.sendWhoPlans()
def land(self):
self.whoplans.value = self.whoplans.OTHER
self.sendWhoPlans()
goal = Goal()
goal.p.x = self.pose.position.x
goal.p.y = self.pose.position.y
goal.p.z = self.pose.position.z
goal.power = True
#Motors still on
print("self.pose.orientation= ", self.pose.orientation)
goal.psi = quat2yaw(self.pose.orientation)
print("goal.yaw= ", goal.psi)
#Note that self.pose.position is being updated in the parallel callback
while (abs(self.pose.position.z - self.alt_ground) > 0.1):
goal.p.z = max(goal.p.z - 0.0035, self.alt_ground)
rospy.sleep(0.01)
self.sendGoal(goal)
#Kill motors once we are on the ground
self.kill()
def kill(self):
self.whoplans.value = self.whoplans.OTHER
self.sendWhoPlans()
goal = Goal()
goal.p.x = self.pose.position.x
goal.p.y = self.pose.position.y
goal.p.z = self.pose.position.z
goal.psi = quat2yaw(self.pose.orientation)
goal.power = False #Turn off the motors
self.sendGoal(
goal
) #TODO: due to race conditions, publishing whoplans.OTHER and then goal.power=False does NOT guarantee that the external planner doesn't publish a goal with power=true