mavcmd - Takeoff from current GPS position

[TSC21]

It doesn't make sense to issue a takeoff from a place different from the current position (unless it's a fixed wing; in that case, I don't know how the control loop works on PX4 for fixed wing; just considering this to a mc).

GPS filtered position retrieved from #87.

Can be issued like this:
rosrun mavros mavcmd takeoff_curr <des_pitch> <yaw> <z>

(Also referenced in here.)

[TSC21]

@vooon can you please tell me if this following piece of code that I'm writing on the mavcmd script will be suitable to do command for the take-off from the current position:

def gp_cb(latitude, longitude):
# don't know how to write the cb

def do_takeoff_curr(args):
    rospy.init_node("mavcmd", anonymous=True)
    rospy.Subscriber("/mavros/global_position/global", NavSatFix, gp_cb)

    try:
        takeoff_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/takeoff_curr", CommandTOL)
        ret = takeoff_cl(min_pitch=args.min_pitch,
                         yaw=args.yaw,
                         latitude=gp_cb.latitude,
                         longitude=gp_cb.longitude,
                         altitude=args.altitude)
    except rospy.ServiceException as ex:
        fault(ex)

    _check_ret(args, ret)

If yes, I'll move to do the others :)

[vooon]

I think service call should be in topic callback function. Also better to add fallback to gps fix message.

[TSC21]

Something like this:

def gps_cb(altitude, longitude):
    try:
        takeoff_curr_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/takeoff_curr", CommandTOL)
        ret = takeoff_curr_cl(min_pitch=args.min_pitch,
                         yaw=args.yaw,
                         latitude=latitude,
                         longitude=longitude,
                         altitude=args.altitude)
        rospy.loginfo("Taking-off from coord: Lat:(%d) Lon:(%d) Alt:(%d) with %f dg of hdg and %f of pitch",
                                latitude, longitude, altitude, yaw, min_pitch)
    except rospy.ServiceException as ex:
        fault(ex)

    _check_ret(args, ret)  

def do_takeoff_curr(args):
    rospy.init_node("mavcmd", anonymous=True)
    rospy.Subscriber("/mavros/global_position/global", NavSatFix, gps_cb)

    rospy.spin()

Also better to add fallback to gps fix message.

What do you mean with this? And how do I pass args to the callback? If you help me on this one, I will be able to write all the other take and land commands, including the local ones. Just need inputs on rospy.

[TSC21]

I'm checking and also do_set_home can use the GPS current position. Isn't there a way of creating a callback to get GPS Fix coordinates as global variables and use them in any of the service calls:
Something like:

def gps_cb(altitude, longitude, altitude):
    global lat
    global lon 
    global alt

    lat = latitude
    lon = longitude
    alt = altitude

And use it in the srv calls like:

def do_takeoff_curr(args):
    rospy.init_node("mavcmd", anonymous=True)
    rospy.Subscriber("/mavros/global_position/global", NavSatFix, gps_cb)

    try:
        takeoff_curr_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/takeoff_curr", CommandTOL)
        ret = takeoff_curr_cl(min_pitch=args.min_pitch,
                         yaw=args.yaw,
                         latitude=lat,
                         longitude=lon,
                         altitude=args.altitude)
        rospy.loginfo("Taking-off from coord: Lat:(%d) Lon:(%d) Alt:(%d) with %f degrees of hdg and %f of pitch",
                                latitude, longitude, altitude, yaw, min_pitch)
    except rospy.ServiceException as ex:
        fault(ex)

    rospy.spin()

    _check_ret(args, ret)

[TSC21]

Update: Well checking this, it may actually work :) What do you think and what else needs corrections?

[vooon]

sethome don't need changes: see option -c, but because limitation of argparse following LLA values needed (set it to 0 0 0).

Also read rospy docs: subscriber pass one argument to callback (message object).
Example: https://github.com/vooon/mavros/blob/master/mavros/scripts/mavwp#L168

[TSC21]

OK the land_curr and takeoff_curr are implemented, but I'm not able to get the script to subscribe to the gps_cb. Here's what I have:

lat = 0
lon = 0
alt = 0

.....

def gps_cb(data): 

    global lat
    global lon
    global alt

    lat = data.latitude
    lon = data.longitude
    alt = data.altitude

.....

def do_takeoff_curr(args):

    global latitude 
    global longitude

    latitude = lat
    longitude = lon

    rospy.init_node("mavcmd", anonymous=True)
    rospy.Subscriber("mavros/global_position/global", NavSatFix, gps_cb)

    rospy.loginfo(latitude) # for debugging purposes
    rospy.loginfo(longitude)

    try:
        takeoff_curr_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/takeoff_curr", CommandTOL)
        ret = takeoff_curr_cl(min_pitch=args.min_pitch,
                         yaw=args.yaw,
                         latitude=latitude,
                         longitude=longitude,
                         altitude=args.altitude)
        rospy.loginfo("Taking-off from coord: Lat:(%d) Lon:(%d) Alt:(%d) with %f dg of hdg and %f of pitch",
                                latitude, longitude, altitude, yaw, min_pitch)
    except rospy.ServiceException as ex:
        fault(ex)

    _check_ret(args, ret) 

.....

def do_land_curr(args):

    global latitude 
    global longitude

    latitude = lat
    longitude = lon

    rospy.init_node("mavcmd", anonymous=True)
    rospy.Subscriber("mavros/global_position/global", NavSatFix, gps_cb)

    rospy.loginfo(latitude)
    rospy.loginfo(longitude)

    try:
        land_curr_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/land_curr", CommandTOL)
        ret = land_curr_cl(min_pitch=0.0,
               yaw=args.yaw,
               latitude=latitude,
               longitude=longitude,
               altitude=args.altitude)
        rospy.loginfo("Landing on current coord: Lat:(%s) Lon:(%s) Alt:(%s) with %s dg of hdg", lat, lon, args.altitude, args.yaw)
    except rospy.ServiceException as ex:
        fault(ex)

    _check_ret(args, ret)

Using the above code I always get lat:0 lon:0 when calling land_curr and takeoff_curr. Supposedly using prefix global in functions should change the global variable, but the problem is it never gets to the callback. Any help here? :)

[TSC21]

Ok the problem apparently was solved by adding rospy.Subscriber("mavros/global_position/global", NavSatFix, gps_cb) to main. But now I get a strange behaviour, where sometimes it gets the current GPS coordinates and sometimes don't, giving lat:0 lon:0. Here's an example if rosruning the command several times:

vision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221891.552444] Landing on current coord: Lat:(0) Lon:(0) Alt:(2.0) with 0.0 dg of hdg
vision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221893.955428] Landing on current coord: Lat:(0) Lon:(0) Alt:(2.0) with 0.0 dg of hdg
vision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221896.192357] Landing on current coord: Lat:(0) Lon:(0) Alt:(2.0) with 0.0 dg of hdg
vision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221898.316991] Landing on current coord: Lat:(0) Lon:(0) Alt:(2.0) with 0.0 dg of hdg
vision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221899.822362] Landing on current coord: Lat:(39.074774) Lon:(-8.8646612) Alt:(2.0) with 0.0 dg of hdg
vision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221945.680527] Landing on current coord: Lat:(39.0746935) Lon:(-8.8647737) Alt:(2.0) with 0.0 dg of hdg
^Cvision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221951.754162] Landing on current coord: Lat:(0) Lon:(0) Alt:(2.0) with 0.0 dg of hdg
^Cvision@ubuntu:~/vision_ros_ws$ rosrun mavros mavcmd land_curr 0.0 2
[INFO] [WallTime: 1408221954.386536] Landing on current coord: Lat:(39.0746787) Lon:(-8.8647706) Alt:(2.0) with 0.0 dg of hdg

Need some guidance to figure this out.

Update: doing some printfs show that this happens cause it sometimes doesn't go to the callback. Same thing happens when subscribing to mavros/fix. Don't now why it happens :S

[TSC21]

Already checked the set_home part. I returned to the previous code. Didn't checked the command description before, sorry.