Hi,
I was trying to write a Python script to control a drone using the keyboard. It seems to work in simulation, but as soon as I try to push it to a physical drone, it does not seem to work properly.
#-*- coding: UTF-8 -*-
import rospy
import tty, termios
import sys, select
from mavros_msgs.msg import AttitudeTarget, PositionTarget, State, ManualControl
from mavros_msgs.srv import CommandBool, SetMode
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import PoseStamped, TwistStamped
from nav_msgs.msg import Odometry
import time
import PyKDL
import math
import numpy as np
from mavros_msgs.srv import CommandLong
class pub:
def __init__(self, uav_type):
self.px = 0
self.py = 0
self.pz = 0
self.vx, self.vy, self.vz = 0, 0, 0
self.roll, self.pitch, self.yaw = 0, 0, 0
self.roll_rate, self.pitch_rate, self.yaw_rate = 0, 0, 0
self.arm_state = False
self.mavros_state = State()
# Obstacle avoidance parameters
self.eta = 0.15 # Repulsive force coefficient
self.d0 = 1.5 # Influence distance for obstacles
self.avoidance_vector_x = 0
self.avoidance_vector_y = 0
self.msg2leader = '''
w: pitch forward
s: pitch backward
a: roll to the left
d: roll to the right
q: yaw to the left
e: yaw to the right
1: arm the drone
8: increse throttle
2: decrese throttle
A: Altitude flight mode
P: Position flight mode
K: kill switch
D: disarm
'''
# Subscribers
self.mavros_sub = rospy.Subscriber(uav_type + "_0/mavros/state", State, self.mavros_state_callback)
self.lidar_sub = rospy.Subscriber(uav_type + "_0/scan", LaserScan, self.scan_cb) # LIDAR scan subscriber
# Publishers
self.target_motion_pub = rospy.Publisher(uav_type + "_0/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
self.body_target_pub = rospy.Publisher(uav_type + "_0/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=2)
self.flightModeService = rospy.ServiceProxy(uav_type + "_0/mavros/set_mode", SetMode)
self.armService = rospy.ServiceProxy(uav_type + "_0/mavros/cmd/arming", CommandBool)
self.manual_control_pub = rospy.Publisher(uav_type + "_0/mavros/manual_control/send", ManualControl, queue_size=10)
#rospy.wait_for_service("/mavros/cmd/command")
self.command_service = rospy.ServiceProxy(uav_type + "_0/mavros/cmd/command", CommandLong)
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
self.arm_state = msg.armed
def arm(self):
if self.armService(True):
print('arm')
return True
else:
print("Vehicle arming failed!")
return False
def disarm(self):
if self.armService(False):
print('disarm')
return True
else:
print("Vehicle disarming failed!")
return False
def takeoff(self):
self.armService(True)
rospy.sleep(1)
self.set_mode('AUTO.TAKEOFF')
def getKey(self):
settings = termios.tcgetattr(sys.stdin)
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def set_mode(self,mode):
rospy.wait_for_service(uav_type + "_0/mavros/set_mode")
try:
set_mode_srv = rospy.ServiceProxy(uav_type + "_0/mavros/set_mode", SetMode)
response = set_mode_srv(custom_mode=mode)
if response.mode_sent:
rospy.loginfo(f"Mode set to {mode}")
else:
rospy.logwarn(f"Failed to set mode to {mode}")
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
# def state_callback(self, msg):
# self.current_state = msg
def terminate_drone(self):
# Try sending MAV_CMD_DO_FLIGHTTERMINATION command
try:
response = self.command_service(False, 185, 0, 1, 0, 0, 0, 0, 0, 0)
if response.success:
rospy.loginfo("Drone termination command sent successfully")
else:
rospy.logerr("Failed to send termination command")
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
def control_drone(self):
rospy.init_node("keyboard_control_drone")
global yaw, pitch, roll, step, throttle
throttle = 500
yaw = 0
pitch = 0
roll = 0
step = 600
self.manual_control_pub.publish(ManualControl(x=0, y=0, z=500, r=0))
while not rospy.is_shutdown():
key = self.getKey()
self.manual_control_pub.publish(ManualControl(x=pitch, y=roll, z=500, r=0))
if key:
if key == 'w': pitch = step
elif key == 's': pitch = -step
elif key == 'a': roll = -step
elif key == 'd': roll = step
elif key == 'q': yaw = step
elif key == 'e': yaw = -step
elif key == '8': throttle = 800
elif key == '2': throttle = -800
elif key == '1': self.arm()
elif key == 'D': self.disarm()
elif key == 't': self.takeoff()
elif key == 'A': self.set_mode('ALTCTL')
elif key == 'P': self.set_mode('POSCTL')
elif key == 'O': self.set_mode('OFFBOARD')
elif key == 'L': self.set_mode('AUTO.LAND')
elif key == 'H': self.set_mode('AUTO.LOITER')
elif key == 'K': self.terminate_drone()
# # Ensure pitch and roll are within allowed bounds
# pitch = max(min(pitch, 1000), -1000)
# roll = max(min(roll, 1000), -1000)
self.manual_control_pub.publish(ManualControl(x=pitch, y=roll, z=throttle, r=yaw))
print(f"throttle = {throttle}, yaw = {yaw}, pitch = {pitch}, roll = {roll}")
rospy.sleep(0.1)
yaw = 0
pitch = 0
roll = 0
throttle =500
self.manual_control_pub.publish(ManualControl(x=pitch, y=roll, z=500, r=0))
yaw = 0
pitch = 0
roll = 0
throttle =500
if __name__ == '__main__':
uav_type = sys.argv[1]
con = pub(uav_type)
con.control_drone()
this is the code that works well in SITL. But as soon as it is put on to a jetson nano connected to the drone, all it does is arm it. No other keys seem to change any attitude of the drone.
Is it possible to have an actual RC connected to the drone (as a backup) and send out attitude commands to the drone using this script running in the companion computer?
will I have to send the command in a different topic or a different Mavlink message?