Ardupilot Circle mode not right in Gazebo

Hi all,

I have been simulating a drone in Gazebo using Mavros with Ardupilot, I've tried different modes and things, but I have a problem with the Circle mode. From the Ardupilot documentation, I understand that without me overwriting yaw control the nose of the drone should always be pointing towards the center of the circle, which is not the case for me. I guess I could write a yaw controller that does that and force it, but there doesn't seem to be an easy way to send yaw controls through mavros in Circle mode (local raw attitude setpoints, if I understand well, can only be used in modes like guided or manual). I don't think I'm overriding anything that could stop the automatic yaw controller, the script is very simple and I only send throttle, which is supposed to be left for the user to control in Circle mode.

Here is my small python script, if anyone has any ideas or has tested the Circle mode in Gazebo and can tell me how it went, that would be great !

#!/usr/bin/env python
import rospy
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import CommandTOL
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from sensor_msgs.msg import NavSatFix
from mavros_msgs.msg import OverrideRCIn
from mavros_msgs.msg import ParamValue
from mavros_msgs.srv import ParamSet

class Operation():

def state_callback(self,data):
self.cur_state = data

def gps_callback(self,data):
self.gps = data

def start_operation(self):
rospy.init_node('my_operation', anonymous=True)
r = rospy.Rate(10)
rospy.Subscriber("/mavros/state", State, self.state_callback)
rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_callback)
pub_rc = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)

r.sleep()

# Service Clients
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
arm = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
takeoff = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)
change_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)

last_request = rospy.get_rostime()

# Change mode to GUIDED
rospy.wait_for_service('/mavros/set_mode')
try:
base_mode = 0
custom_mode = "GUIDED"
out = change_mode(base_mode, custom_mode)
if out.success:
else:
except rospy.ServiceException, e:
last_request = rospy.get_rostime()

while not out.success:
r.sleep()
out = change_mode(base_mode, custom_mode)
if out.success:
else:

# Arm drone
rospy.wait_for_service('/mavros/cmd/arming')
try:
out = arm(True)
if out.success:
else:
except rospy.ServiceException, e:
last_request = rospy.get_rostime()

while not out.success:
r.sleep()
out = arm(True)
if out.success:
else:

# Take off
current_altitude = self.gps.altitude
rospy.wait_for_service('/mavros/cmd/takeoff')
try:
min_pitch = 0
yaw = 0
latitude = 0 #self.gps.latitude
longitude = 0 #self.gps.longitude
altitude = 4
out = takeoff(min_pitch, yaw, latitude, longitude, altitude)
if out.success:
rospy.loginfo("Failed taking-off ...