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 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 kill(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.cut_power = True
self.sendGoal(goal)
self.mode.mode = self.mode.ON_GROUND
self.sendMode()
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.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 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):
goal=Goal();
goal.p.x = self.pose.position.x;
goal.p.y = self.pose.position.y;
goal.p.z = self.pose.position.z;
#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);
self.sendGoal(goal)
#Kill motors once we are on the ground
self.kill()
def __init__(self):
self.state = State()
self.state.pos.x = rospy.get_param('~x', 0.0)
self.state.pos.y = rospy.get_param('~y', 0.0)
self.state.pos.z = rospy.get_param('~z', 0.0)
self.state.quat.x = 0
self.state.quat.y = 0
self.state.quat.z = 0
self.state.quat.w = 1
self.current_yaw = 0.0
#Publishers
self.pubCmdVel = rospy.Publisher('jackal_velocity_controller/cmd_vel',
Twist,
queue_size=1,
latch=True)
self.pubState = rospy.Publisher('state',
State,
queue_size=1,
latch=False)
#Timers
self.timer = rospy.Timer(rospy.Duration(0.1), self.cmdVelCB)
self.kv = 1.0
self.kdist = 2.5 #0.8
self.kw = 1.0
self.kyaw = 2.0
self.kalpha = 1.5
self.state_initialized = False
self.goal_initialized = False
self.goal = Goal()
self.goal.p.x = 0.0
self.goal.p.y = 0.0
self.goal.p.z = 0.0
self.goal.v.x = 0.0
self.goal.v.y = 0.0
self.goal.v.z = 0.0
self.goal.a.x = 0.0
self.goal.a.y = 0.0
self.goal.a.z = 0.0
self.state_initialized = False
def kill(self):
goal = Goal()
goal.cut_power = True
self.sendGoal(goal)
self.mode.mode = self.mode.ON_GROUND
self.sendMode()
def kill(self):
goal=Goal();
goal.power=False
self.sendGoal(goal)
self.mode.mode=self.mode.ON_GROUND
self.sendMode()
