Problem With Changing Flight Mode GUIDED to AUTO

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.


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: 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!