Ardupilot Circle mode not right in Gazebo

edit

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
    self.gps_read = True

def start_operation(self):
    rospy.init_node('my_operation', anonymous=True) 
    self.gps_read = False
    self.localtarget_received = False
    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)

    while not self.gps_read:
        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:
            rospy.loginfo("GUIDED mode set")
        else:
            rospy.loginfo("Failed SetMode")
    except rospy.ServiceException, e:
        rospy.loginfo("Service call failed")
    last_request = rospy.get_rostime() 

    while not out.success:
        r.sleep()
        out = change_mode(base_mode, custom_mode)
        if out.success:
            rospy.loginfo("setmode send ok value")
        else:
            rospy.loginfo("Failed SetMode")

    # Arm drone
    rospy.wait_for_service('/mavros/cmd/arming')
    try:
        out = arm(True)
        if out.success:
            rospy.loginfo("Armed")
        else:
            rospy.loginfo("Failed Arming")
    except rospy.ServiceException, e:
        rospy.loginfo("Service call failed")
    last_request = rospy.get_rostime() 

    while not out.success:
        r.sleep()
        out = arm(True)
        if out.success:
            rospy.loginfo("Armed")
        else:
            rospy.loginfo("Failed Arming")

    # 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("Took-off")
        else:
            rospy.loginfo("Failed taking-off ...