def takeoff_cmd(self, target):
rospy.loginfo("Sending takeoff")
cmd = drone_onboard_command()
cmd.command_type = drone_onboard_command.CTRL_TAKEOF_COMMAND
cmd.param1 = 10000
msg = self.drone_onboard_cmd_tomav(target, cmd)
self.send_mavlink_msg(msg)
def landing_cmd(self, target, att_landing=True):
rospy.loginfo("Sending landing")
cmd = drone_onboard_command()
cmd.command_type = drone_onboard_command.CTRL_LANDING_COMMAND
if att_landing:
cmd.param1 = 1
else:
cmd.param1 = 0
msg = self.drone_onboard_cmd_tomav(target, cmd)
self.send_mavlink_msg(msg)
def toggle_arm_disarm(self, target, arm=True):
# msg = self.mav.command_long_encode(target, 0, pymavlink.MAV_CMD_DO_SET_MODE, 0, 192, 0, 0, 0, 0, 0, 0)
cmd = drone_onboard_command()
cmd.command_type = drone_onboard_command.CTRL_ARM_COMMAND
if arm:
cmd.param1 = 1
rospy.loginfo("Sending ARM")
else:
cmd.param1 = 0
rospy.loginfo("Sending DISARM")
msg = self.drone_onboard_cmd_tomav(target, cmd)
self.send_mavlink_msg(msg)
rospy.init_node('cmded', anonymous=True)
print("Sending to onboard")
pub = rospy.Publisher("/drone_commander/onboard_command",
drone_onboard_command,
queue_size=1)
rate = rospy.Rate(50) # 20hz
while not rospy.is_shutdown():
connections = pub.get_num_connections()
print("Wait for pub")
if connections > 0:
break
rate.sleep()
cmd = drone_onboard_command()
if args.command_type == "takeoff":
if len(args.params) < 1:
rospy.logwarn("No height specs, will fly to default 1.0m")
height = 1.0
else:
height = args.params[0]
cmd.command_type = drone_onboard_command.CTRL_TAKEOF_COMMAND
cmd.param1 = int(height * 10000)
send(cmd, args, pub)
elif args.command_type == "landing":
cmd.command_type = drone_onboard_command.CTRL_LANDING_COMMAND
cmd.param1 = 0
cmd.param2 = 3000
def simple_mission_callbck(self, e):
if self.in_mission is None:
return
dt = 0
if e.last_real is not None:
dt = (e.current_real - e.last_real).to_sec()
t = (e.current_real - self.start_misssion_time).to_sec()
rospy.loginfo_throttle(1.0, "Act mission {} with {:5.3f}s".format(self.in_mission, t))
def mission_A(t):
ox = 0
oy = 0
r = 0.5
T = 15
x = ox + math.sin(t*math.pi*2/T)*r
y = oy + math.cos(t*math.pi*2/T)*r
vx = math.cos(t*math.pi*2/T) * r * math.pi*2/T
vy = -math.sin(t*math.pi*2/T) * r * math.pi*2/T
return x, y, 1, vx, vy, 0
def mission_B(t):
ox = 0
oy = 0
r = 0.5
T = 15
x = ox + math.sin(t*math.pi*2/T)*r
y = oy + math.sin(2*t*math.pi*2/T)*r
vx = math.cos(t*math.pi*2/T) * r * math.pi*2/T
vy = 2*math.cos(2*t*math.pi*2/T) * r * math.pi*2/T
return x, y, 1, vx, vy, 0
def mission_Y(t):
ox = 0
oy = 0
oz = 1
r = 0.5
T = 15
x = ox + math.sin(t*math.pi*2/T)*r
y = oy + math.sin(2*t*math.pi*2/T)*r
z = oz + math.sin(2*t*math.pi*2/T+math.pi)*0.3
vx = math.cos(t*math.pi*2/T) * r * math.pi*2/T
vy = 2*math.cos(2*t*math.pi*2/T) * r * math.pi*2/T
vz = 2*math.cos(2*t*math.pi*2/T + math.pi) * 0.3 * math.pi*2/T
return x, y, z, vx, vy, vz
x, y, z, vx, vy, vz = 0, 0, 0, 0, 0, 0
if self.in_mission == "A":
x, y, z, vx, vy, vz = mission_A(t)
if self.in_mission == "B":
x, y, z, vx, vy, vz = mission_B(t)
if self.in_mission == "Y":
x, y, z, vx, vy, vz = mission_Y(t)
cmd = drone_onboard_command()
cmd.command_type = drone_onboard_command.CTRL_POS_COMMAND
cmd.param1 = int(x*10000)
cmd.param2 = int(y*10000)
cmd.param3 = int(z*10000)
cmd.param4 = 666666
cmd.param5 = int(vx*10000)
cmd.param6 = int(vy*10000)
cmd.param7 = int(vz*10000)
cmd.param8 = 0
cmd.param9 = 0
cmd.param10 = 0
msg = self.drone_onboard_cmd_tomav(self.target_id, cmd)
print(msg)
self.send_mavlink_msg(msg)