Servers by jDrones

Problem With Changing Flight Mode GUIDED to AUTO

Task:
I have the task to make a drone autonomous, the idea is to make a drone move to waypoints using AUTO mode with Python, rospy, ArduPilot SITL, Mavros, Mavlink and Gazebo 9. ROS melodic is used. All in Linux 18.04. No map.

Code:

Problem:
The problem is when I use guided mode to take off, I cannot change the mode with Python from GUIDED to AUTO.
When I use MAV_CMD_NAV_TAKEOFF ([22]) to takeoff the drone, when loading waypoints , the first waypoint get SITL_START_LOCATION overwritten by from: https://github.com/ardupilot/ardupilot/blob/master/Tools/autotest/arducopter.py#L3382 meaning the drone doesnt takeoff as the first command is a 16 and not a 22 command. When takeoff using FBW in ArduPilot SITL terminal, the UAV flies to somewhere fly which I’m assuming is the SITL_START_LOCATION.
The specific failure in MAVLink I get when trying to change the mode from GUIDED to AUTO is “APM: Flight mode change failed Got MAVLink msg: COMMAND ACK{Command:11, result:3}”

Thank you in Advanced, any help would be much appreciated!

Servers by jDrones