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](http://ardupilot.org/copter/docs/circle-mode.html), 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 a similar problem (that way I could know if it's just a Gazebo bug or if I'm doing something wrong), 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")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
# Keep sending take-off messages until received by FCU
while not out.success:
r.sleep()
out = takeoff(min_pitch, yaw, latitude, longitude, altitude)
if out.success:
rospy.loginfo("Took-off")
else:
rospy.loginfo("Failed taking-off")
while self.gps.altitude< current_altitude+altitude-0.1:
r.sleep()
differ = self.gps.altitude - current_altitude
rospy.loginfo("Waiting to take off, current height %s", differ)
# Change GCS parameter
rospy.wait_for_service('/mavros/param/set')
try:
myparam = ParamValue()
myparam.integer = 1
out = change_param("SYSID_MYGCS", myparam)
if out.success:
rospy.loginfo("Changed my GCS")
else:
rospy.loginfo("Failed changing GCS")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
# Set throttle to 1500 (not controlled in circle mode)
myrc = OverrideRCIn()
myrc.channels[2] = 1500
pub_rc.publish(myrc)
rospy.sleep(3.)
# Change GCS parameter back
try:
rospy.wait_for_service('/mavros/param/set')
myparam = ParamValue()
myparam.integer = 255
myparam.real = 0
out = change_param("SYSID_MYGCS", myparam)
if out.success:
rospy.loginfo("Changed my GCS back")
else:
rospy.loginfo("Failed changing GCS")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
# Change RTL parameters
rospy.wait_for_service('/mavros/param/set')
try:
myparam.integer = 0
myparam.real = 4.0
out = change_param("RTL_ALT", myparam)
if out.success:
rospy.loginfo("Changed rtl altitude")
else:
rospy.loginfo("Failed changing rtl altitude")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
# Change circle parameters
rospy.wait_for_service('/mavros/param/set')
try:
myparam.integer = 0
myparam.real = 1.0 # now in deg/s!
out = change_param("CIRCLE_RATE", myparam)
if out.success:
rospy.loginfo("Changed circle rate")
else:
rospy.loginfo("Failed changing circle rate")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
try:
myparam.integer = 0
myparam.real = 1000.0 # now in cm!
out = change_param("CIRCLE_RADIUS", myparam)
if out.success:
rospy.loginfo("Changed circle radius")
else:
rospy.loginfo("Failed changing circle radius")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
# Change mode to circle
rospy.wait_for_service('/mavros/set_mode')
try:
rospy.sleep(10.)
base_mode = 0
custom_mode = "CIRCLE"
out = change_mode(base_mode, custom_mode)
if out.success:
rospy.loginfo("CIRCLE mode set")
else:
rospy.loginfo("Failed setting CIRCLE mode")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
# Change GCS parameter back
rospy.wait_for_service('/mavros/param/set')
try:
myparam = ParamValue()
myparam.integer = 255
myparam.real = 0
out = change_param("SYSID_MYGCS", myparam)
if out.success:
rospy.loginfo("Changed my GCS back")
else:
rospy.loginfo("Failed changing GCS")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
if __name__ == '__main__':
my_operation = Operation()
my_operation.start_operation()
↧