Hi,
I am trying to write a script that allows the user to control a simulated drone (simulated in Gazebo) using the ‘w’, ‘a’, ‘s’, and ‘d’ keys. My main code gets the keypress by subscribing to a publisher node that takes in the user’s keypress.
My drone is taking off to 1 meter, but as soon as I hit any key to change the attitude of the drone, the drone drops to the floor and then takes the change in attitude. the drone is responding to the change in attitude but the altitude is not being maintained. My code is structured in a way that the altitude should be maintained first then the attitude change should take place, but the drone is still dropping to the floor as soon as a key is pressed.
This is my main code
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
import asyncio
from mavsdk import System
from mavsdk.offboard import Attitude, OffboardError, PositionNedYaw
key_pressed = None # Variable to store the key press
# Callback function for the keyboard input
def keyboard_callback(data):
global key_pressed
key_pressed = data.data
# Drone control logic
async def control_drone(drone):
global key_pressed
# Hover at 1 meter initially
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -1.0, 0.0))
await asyncio.sleep(2) # Hover for 2 seconds
while not rospy.is_shutdown():
if key_pressed:
key = key_pressed
if key in ['w', 's', 'a', 'd', 'q', 'e']:
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -1.0, 0.0)) # Maintain 1 meter altitude
if key == 'w':
await drone.offboard.set_attitude(Attitude(30.0, 0.0, 0.0, 0.6))
await asyncio.sleep(1)
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.6))
elif key == 's':
await drone.offboard.set_attitude(Attitude(-30.0, 0.0, 0.0, 0.6))
await asyncio.sleep(1)
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.6))
elif key == 'a':
await drone.offboard.set_attitude(Attitude(0.0, 30.0, 0.0, 0.6))
await asyncio.sleep(1)
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.6))
elif key == 'd':
await drone.offboard.set_attitude(Attitude(0.0, -30.0, 0.0, 0.6))
await asyncio.sleep(1)
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.6))
elif key == 'q':
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 30.0, 0.6))
await asyncio.sleep(1)
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.6))
elif key == 'e':
await drone.offboard.set_attitude(Attitude(0.0, 0.0, -30.0, 0.6))
await asyncio.sleep(1)
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.6))
key_pressed = None # Reset the key_pressed variable after processing
await asyncio.sleep(0.1) # Adjust the sleep time as needed
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print("-- Connected to drone!")
break
print("Waiting for drone to have a global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok and health.is_home_position_ok:
print("-- Global position estimate OK")
break
print("-- Arming")
await drone.action.arm()
#print("-- Taking off")
#await drone.action.takeoff()
print("-- Setting initial setpoint")
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 1))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(f"Starting offboard mode failed with error code: \
{error._result.result}")
print("-- Disarming")
await drone.action.disarm()
return
# ROS Node initialization and subscriber
rospy.init_node("keyboard_subscriber")
rospy.Subscriber("keyboard_input", String, keyboard_callback)
try:
await control_drone(drone) # Control the drone based on subscribed key presses
except KeyboardInterrupt:
print("\nExiting due to Ctrl+C")
finally:
await drone.offboard.stop()
await drone.action.land()
if __name__ == "__main__":
asyncio.run(run())
This is my Keypress publisher node
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
import sys, select, termios, tty
def getKey():
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
if __name__ == "__main__":
rospy.init_node("keyboard_publisher")
key_pub = rospy.Publisher("keyboard_input", String, queue_size=10)
settings = termios.tcgetattr(sys.stdin)
rate = rospy.Rate(10) # Adjust as needed
print("Listening for keyboard input... Press down 'Ctrl+C' for more than 1 sec to exit.")
try:
while not rospy.is_shutdown():
key = getKey()
if key:
rospy.loginfo(f"Key pressed: {key}")
key_pub.publish(key)
rate.sleep()
except KeyboardInterrupt:
print("\nExiting due to Ctrl+C")
I use the “make px4_sitl gazebo” to bring up the drone simulation in Gazebo and to have PX4 as SITL. And I am working with ROS1 in a Ubuntu 20.04 environment.
Could anyone please help me to understand what i am doing wrong?
