pose.pose.position.x = 0.8
global i
t0 = rospy.Time.now().to_sec(
) # To just wander left and ri8 to get a detection
while rospy.Time.now().to_sec() - t0 < 0.2: # its enough i think
# if i%2 == 0:
# pose.pose.position.y = 1
# else:
# pose.pose.position.y = -1
pose.pose.position.y = 0.2
# if current_position.pose.position.z > 3.28:
# pose.pose.position.z = 3.27
# elif current_position.pose.position.z < 3.27:
# pose.pose.position.z = 3.28
pose.pose.position.z = 3
pose.pose.orientation.w = 1
local_pos_pub.publish(pose)
i = i + 1
if __name__ == '__main__':
try:
take = armtakeoff()
take.arm()
take.takeoff()
main()
except rospy.ROSInterruptException:
pass
i = i+1
main(i)
elif dist <0.0005 and i>=len(msg.points):
i=0
msg =0
while msg==0:
rospy.Subscriber('/mav_local_planner/full_trajectory',MultiDOFJointTrajectory,callback)
print("no hope")
print("msg not zero")
print(msg)
main(i)
if __name__ == '__main__':
try:
armclass = armtakeoff()
# global setpoint
print(setpoint)
setpoint_pub = rospy.Publisher('/waypoint', PoseStamped,queue_size=10)
setpoint_pub.publish(setpoint)
# rospy.sleep(1)
rospy.Subscriber('/mav_local_planner/full_trajectory',MultiDOFJointTrajectory,callback)
while msg==0:
rospy.Subscriber('/mav_local_planner/full_trajectory',MultiDOFJointTrajectory,callback)
print("no hope")
print("msg not zero")
print(msg)
from armf import armtakeoff
import rospy
rospy.init_node('do_move1', anonymous=True)
a = armtakeoff()
a.arm()
a.takeoff1()
def main():
pygame.init()
screen = pygame.display.set_mode((640, 480))
clock = pygame.time.Clock()
x_curser1 = 160
y_curser1 = 240
x_curser2 = 480
y_curser2 = 240
mode = ''
count = 0
radius = 10
screen.fill((0, 0, 0))
color_curser1 = (0, 255, 0)
color_curser2 = (0, 255, 0)
pygame.draw.circle(screen, color_curser1, (x_curser1, y_curser1), radius)
pygame.draw.circle(screen, color_curser2, (x_curser2, y_curser2), radius)
arm = armtakeoff()
arm.arm()
arm.takeoff()
while True:
screen.fill((0, 0, 0))
x_curser1 = 160
y_curser1 = 240
x_curser2 = 480
y_curser2 = 240
pressed = pygame.key.get_pressed()
alt_held = pressed[pygame.K_LALT] or pressed[pygame.K_RALT]
ctrl_held = pressed[pygame.K_LCTRL] or pressed[pygame.K_RCTRL]
for event in pygame.event.get():
# determin if X was clicked, or Ctrl+W or Alt+F4 was used
if event.type == pygame.QUIT:
return
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_w and ctrl_held:
return
if event.key == pygame.K_F4 and alt_held:
return
if event.key == pygame.K_ESCAPE:
return
# determine if a letter key was pressed
if event.key == pygame.K_w:
mode = 'up'
elif event.key == pygame.K_s:
mode = 'down'
elif event.key == pygame.K_d:
mode = 'right'
elif event.key == pygame.K_a:
mode = 'left'
elif event.key == pygame.K_SPACE:
mode = 'jump'
elif event.key == pygame.K_q:
mode = 'yaw'
elif event.key == pygame.K_e:
mode = 'yawri8'
elif event.key == pygame.K_LCTRL:
mode = 'low'
elif event.key == pygame.K_h:
mode = 'hold'
color_curser1 = (255, 0, 0)
color_curser2 = (255, 0, 0)
color_curser3 = (0, 0, 255)
color_curser4 = (0, 0, 255)
pygame.draw.circle(screen, color_curser1, (x_curser1, y_curser1),
radius)
pygame.draw.circle(screen, color_curser2, (x_curser2, y_curser2),
radius)
x_curser1, y_curser1, x_curser2, y_curser2 = curserControl(
screen, x_curser1, y_curser1, x_curser2, y_curser2, mode, count,
color_curser1, color_curser1, radius)
pygame.draw.circle(screen, color_curser3, (x_curser1, y_curser1),
radius)
pygame.draw.circle(screen, color_curser4, (x_curser2, y_curser2),
radius)
pygame.display.flip()
clock.tick(60)