SITL Script Fixes

[dowonlee777]

Here is my rough list of to-dos from our meeting. Please feel free to add to or edit the list

Related to getHomeLocation():

• Make sure we have a GPS fix before trying to get the home location. Use dronekit.GPSInfo to check fix_type == 3.
  • The sitl_basic.py script now checks this value
• FUTURE: Include a time limit for getting the home location. Maybe 30 seconds in loop before exiting?
  • We'll need a way of communicating this failure (e.g., via a ROS message).
  • Let's wait to work on this until we start doing ROS things again.

Establishing Threads:

• Put mission monitor while loop in a thread:
  • Pause at the end of the loop with rospy.rate sleep instead of time.sleep
• FUTURE: We'll also put the telemetry message publication into a separate thread.

Related to Geofence:

• Write a function that will check if a point is within a polygon.
  • I implemented this as geoIsPointInPoly() at the top of the sitl_basic.py function.
    • Might want to move this to a new file (like distance_functions.py or common.py).
  • See the VeRoViz function isPointInPoly (https://veroviz.org/docs/_modules/veroviz/utilities.html#isPointInPoly)
  • Also see https://github.com/optimatorlab/sttr/blob/master/catkin_ws_pi/sttr_pi/scripts/distance_functions.py
• When establishing the home location, make sure it's within a geofence (polygon).
  • There's now a check for this within the getHomeLocation() function.
• In the main mission/control thread, check if drone is leaving geofence.
• Create a "simple" Cesium app that will allow us to quickly/easily define a geofence:
  • Allow user to click on the map to define the vertices of the polygon.
  • At each vertex, capture the altitude (meters above mean sea level, MSL)
  • Write the vertices as a list of lists (and automatically copy to the user's clipboard). The format would be like:

    GEOFENCE = [[43.086441866511805, -78.84681701660156],
                             [42.98003720570486, -78.84475708007812],
                             [42.97953485978903, -78.68133544921876],
                             [43.090453524374716, -78.68545532226564]]
    

    • This will make it easy to copy the geofence to the sitl_basic.py script.
  • How do we want to capture the altitude??? As a separate list? Should we add alt to the geofence vertices (i.e., make it[lat, lon, alt] for each vertex)? This will require us to modify the point-in-poly function (which isn't a big deal).
• Add an altitude check to the geofence check. (this will also apply to establishing the home location).
  • Maybe we should write a "checkGeofence" function, since it looks like it will be called both during pre-arm and in-flight.
• FUTURE: Migrate the "simple" Cesium app's functionality to be included in m3c.
• FUTURE: The geofence coordinates should be provided by the GCS (as an input). Right now, the coordinates are hard-coded.

Pre-Arm Stuff

• Does our code truly hang when running dronekit's pre-arm checks?
  • If not, let's just re-implement the checks.
  • If yes:
    • Make a list of all of dronekit's pre-arm checks.
    • Can we manually check each of these? Which one(s) is/are failing?
      • If a particular check is being failed and isn't critical, we may decide to continue disabling pre-arm checks

Other Future Items:

• FUTURE: Check the GPS fix_type during the execution of the mission (to make sure we aren't losing GPS signal).

  • This would go within our main control thread (the one that checks if we've completed our current task).
  • We'll need a way of communicating this failure (e.g., via a ROS message).

• FUTURE: Check where we need clearCmds(), pass in length of cmds in ROS status msg

---

FYI

• Do we need:

   cmds = vehicle[vehicleIndex].commands
   cmds.download()
   cmds.wait_ready()
  

  • Yes. These commands are necessary to allow us to check the vehicle's home location. I just found this in the dronekit documentation (https://dronekit-python.readthedocs.io/en/latest/automodule.html):

    To get the attribute you must first download the Vehicle.commands(). The attribute has a value of None until Vehicle.commands() has been downloaded and the autopilot has set an initial home location (typically where the vehicle first gets GPS lock).

[cmurray3]

Thanks, Dowon! I made some edits in the original post above.

Also, I have posted the revised sitl_basic.py script. It now has some basic geofence functionality (checking if point is within polygon) and has modified the way we establish the home location.