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 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 land(self):
goal = Goal()
goal.pos.x = self.pose.position.x
goal.pos.y = self.pose.position.y
goal.pos.z = self.pose.position.z
goal.yaw = quat2yaw(self.pose.orientation)
#Note that self.pose.position is being updated in the parallel callback
while (abs(self.pose.position.z - self.alt_ground) > 0.1):
goal.pos.z = max(goal.pos.z - 0.0035, self.alt_ground)
self.sendGoal(goal)
#Kill motors once we are on the ground
self.kill()
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.yaw = quat2yaw(self.pose.orientation)
#Note that self.pose.position is being updated in the parallel callback
######## Commented for simulations
# while( abs(self.pose.position.z-self.alt_taken_off)>0.1 ):
# goal.pos.z = min(goal.pos.z+0.0035, self.alt_taken_off);
# #rospy.sleep(0.004)
# self.sendGoal(goal)
########
rospy.sleep(0.1)
self.mode.mode = self.mode.GO
self.sendMode()
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.yaw = quat2yaw(self.pose.orientation)
print("goal.yaw= ", goal.yaw)
#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()