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 pubCB(self, timer):
goal=Goal()
goal.header.stamp=rospy.Time.now();
ti=self.t0+(rospy.get_time()-self.time_end_init); #Same as before, but it's float
whole_traj_ti=self.evalSimpyArray(ti, self.whole_traj)
whole_traj_dot_ti=self.evalSimpyArray(ti, self.whole_traj_dot)
whole_traj_dot2_ti=self.evalSimpyArray(ti, self.whole_traj_dot2)
whole_traj_dot3_ti=self.evalSimpyArray(ti, self.whole_traj_dot3)
goal.p.x=whole_traj_ti[0]
goal.p.y=whole_traj_ti[1]
goal.p.z=whole_traj_ti[2]
goal.v.x=whole_traj_dot_ti[0]
goal.v.y=whole_traj_dot_ti[1]
goal.v.z=whole_traj_dot_ti[2]
goal.a.x=whole_traj_dot2_ti[0]
goal.a.y=whole_traj_dot2_ti[1]
goal.a.z=whole_traj_dot2_ti[2]
goal.j.x=whole_traj_dot3_ti[0]
goal.j.y=whole_traj_dot3_ti[1]
goal.j.z=whole_traj_dot3_ti[2]
goal.yaw = 0.0
goal.power= True; #Turn on the motors
if(self.whoplans.value==self.whoplans.PANTHER):
self.pubGoal.publish(goal)
else:
#This case should only happen when I've entered whoplansCB while I was at the same time in pubCB. See comment in comands.py about race conditions.
pass
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.yaw = 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
def takeOff(self):
goal=Goal();
goal.power=True
goal.p.x = self.pose.position.x;
goal.p.y = self.pose.position.y;
goal.p.z = self.pose.position.z;
if(self.is_ground_robot==True):
self.alt_taken_off=self.pose.position.z;
#Note that self.pose.position is being updated in the parallel callback
while( abs(self.pose.position.z-self.alt_taken_off)>0.1 ):
goal.p.z = min(goal.p.z+0.0035, self.alt_taken_off);
#rospy.sleep(0.004) #TODO hard-coded
self.sendGoal(goal)
rospy.sleep(1.5)
self.mode.mode=self.mode.GO
self.sendMode();
def kill(self):
goal=Goal();
goal.power=False
self.sendGoal(goal)
self.mode.mode=self.mode.ON_GROUND
self.sendMode()