relative positioning updates

Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h

@@ -70,9 +70,18 @@ typedef struct ServiceAck
 } ServiceAck;
 
 
+float eucDistance(std::vector<float> a, std::vector<float> b);
+std::vector<std::vector<float>> rotateAxisAngle(std::vector<float> u, float angleRad);
+float dotProduct(std::vector<float> vect_A, std::vector<float> vect_B);
+std::vector<float> crossProduct(std::vector<float> vect_A, std::vector<float> vect_B);
+std::vector<float> normalizeVector(std::vector<float> vec);
+float Magnitude3(std::vector<float> v);
+std::vector<float> MatrixMultiply(std::vector <std::vector<float>> a, std::vector<float> b);
+std::vector<std::vector<float>> GetRotationMatrixParameter();
 
 bool setupWaypointMission(int responseTimeout);
 bool runWaypointMission();
+bool runPositionMission();
 
 void setWaypointDefaults(DJI::OSDK::WayPointSettings* wp);
 
@@ -101,12 +110,24 @@ ServiceAck obtainCtrlAuthority();
 
 bool takeoff();
 
+bool relativePosition(float x, float y, float z, float yaw);
+
 bool land();
 
+bool setRates();
+
+bool stopMotion();
+
+bool goHome();
+
+void calibratePositionReference();
+
+std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec);
+
 bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res);
 
 void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
-//void waypointEventCallback(Vehicle *vehicle, RecvContainer recvFrame, UserData userData);
+void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg);
 
 
 #endif // MISSION_NODE_H

Onboard-SDK-ROS/launch/local_test.launch

@@ -12,8 +12,27 @@
     <param name="use_broadcast" type="bool" value="false"/>
     </node>
     <param name="base_speed" type="double" value="0.5"/>
+    <param name="min_speed" type="double" value="0.1"/>
+    <param name="speed_scalar" type="double" value="0.01"/>
     <param name="drone_id" type="int" value="0"/>
     <param name="camera_type" type="int" value="1"/>
+    <!-- <param name="angle_offset" type="double" value="-33"/> -->
+
     <node pkg="dji_osdk_ros" type="bus_driver" name="bus_driver" output="screen"/>
+    <node pkg="dji_osdk_ros" type="poseFeedbackConversion.py" name="poseFeedbackConversion" output="screen">
+      <rosparam command="load" param="angleOffset">
+        -33.0
+      </rosparam>
+    </node>
+
+    <node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros">
+      <rosparam command="load" param="anchorPoint">
+          [37.196986, -80.578257]
+      </rosparam>
+      <rosparam command="load" param="altitude">
+           2.0
+      </rosparam>
+    </node>
+
     <!-- <node pkg="dji_osdk_ros" type="image_saver.py" name="image_saver" output="screen"/> -->
 </launch>

Onboard-SDK-ROS/scripts/image_activation.py

-#! /usr/bin/python3
+#!/usr/bin/python3
 
 # Copyright (c) 2015, Rethink Robotics, Inc.
 

Onboard-SDK-ROS/scripts/poseFeedbackConversion.py

+#!/usr/bin/python3
+
+import rospy
+import numpy as np
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import PointStamped
+from dji_osdk_ros.msg import VOPosition
+from geometry_msgs.msg import Transform
+from geometry_msgs.msg import Vector3
+from geometry_msgs.msg import Quaternion
+from scipy.spatial.transform import Rotation
+from tools import lat_lon2meters, meters2lat_lon
+import math
+
+homeSetFlag = False
+# homePoint = PointStamped()
+homePoint = NavSatFix()
+transform = Transform()
+angleOffset = 0.0
+
+def point_rotation(origin, pt, ang):
+    # returns the pt rotated about the origin by ang (ang in degrees)
+    c = math.cos(math.radians(ang))
+    s = math.sin(math.radians(ang))
+    # translate to origin
+    pt_temp = [pt[0] - origin[0], pt[1] - origin[1]]
+    pt_spun = [ pt_temp[0]*c - pt_temp[1]*s, pt_temp[0]*s + pt_temp[1]*c ]
+    # translate back to frame
+    pt_spun = [pt_spun[0] + origin[0], pt_spun[1] + origin[1]]
+    return pt_spun
+
+
+
+def callback(data):
+    global homeSetFlag
+    global homePoint
+    global transform
+    if not homeSetFlag: # mildly hacky, depends on position format we're using for feedback. might build into bus driver and pub from there
+        # set up home point in yee pee es
+        homePoint = data
+
+        # homePoint.point.x = data.x
+        # homePoint.point.y = data.y
+        # homePoint.point.z = data.z
+        # homePoint.header = data.header
+        homeSetFlag = True
+        rospy.loginfo("setting home point...")
+
+    # convert and compare against home point
+    translation = Vector3()
+    rotation = Quaternion()
+
+    # rotate the xy plane by angle
+    # dataPointSpun = point_rotation([0, 0], [data.x, data.y], angleOffset)
+
+    homePointMeters = lat_lon2meters([homePoint.latitude, homePoint.longitude])
+    currentPointMeters = lat_lon2meters([data.latitude, data.longitude])
+
+    currentOffset = [currentPointMeters[0] - homePointMeters[0], currentPointMeters[1] - homePointMeters[1]]
+
+
+    # angleOffset = -33.0
+    # rospy.loginfo(angleOffset)
+    # rospy.loginfo(currentOffset)
+    angleOffset = rospy.get_param("~angleOffset")
+    currentRotatedOffset = point_rotation([0,0], currentOffset, angleOffset)
+    # rospy.loginfo(currentRotatedOffset)
+
+    translation.x = currentRotatedOffset[1]
+    translation.y = currentRotatedOffset[0]
+    translation.z = data.altitude - homePoint.altitude
+
+    transform.translation = translation
+
+    rot = Rotation.from_euler('xyz', [0, 0, 0], degrees = True) # be not rotated pls
+    zeroQuaternion = rot.as_quat()
+    rotation.x = zeroQuaternion[0]
+    rotation.y = zeroQuaternion[1]
+    rotation.z = zeroQuaternion[2]
+    rotation.w = zeroQuaternion[3]
+
+    transform.rotation = rotation
+
+def main():
+    global transform
+    rospy.init_node("poseFeedbackConversion")
+    rospy.Subscriber("dji_osdk_ros/gps_position", NavSatFix, callback, queue_size = 1)
+    posePublisher = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
+    while not rospy.is_shutdown():
+        posePublisher.publish(transform)
+
+
+if __name__ == '__main__':
+    main()

Onboard-SDK-ROS/scripts/tools.py

+#!/usr/bin/python3
+from pyproj import Transformer
+
+def meters2lat_lon(meters_point):
+    # returns point converted to lat lon from meters coordinate system
+    transformer = Transformer.from_crs(3857, 4326)
+    latlon_point = transformer.transform(meters_point[0], meters_point[1])
+    return latlon_point
+
+def lat_lon2meters(latlon_point):
+    # returns point converted to meters from lat lon coordinate system
+    transformer = Transformer.from_crs(4326, 3857)
+    meters_point = transformer.transform(latlon_point[0], latlon_point[1])
+    return meters_point

Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py

+#!/usr/bin/python3
+from tools import meters2lat_lon, lat_lon2meters
+import numpy as np
+import rospy
+
+
+def normalizeAngle(angle):
+    newAngle = angle%360
+    newAngle = (newAngle + 360)%360
+    if(newAngle >= 180):
+        newAngle -= 360
+    return newAngle
+
+def generateWaypoints():
+
+    rospy.init_node("waypointGenerator")
+    anchorPoint = rospy.get_param("~anchorPoint")
+    waypointAltitude = rospy.get_param("~altitude")
+
+    # ring sizes
+    big = 10
+    med = 5
+    sml = 3
+
+    apMeters = lat_lon2meters(anchorPoint)
+
+    waypointOffsets = [
+    [-big, -big],
+    [big, -big],
+    [big, big],
+    [med, med],
+    [-med, med],
+    [-med, -med],
+    [-sml, -sml],
+    [sml, -sml],
+    [sml, sml],
+    [big, -big],
+    ]
+    tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0}
+    waypointDict = {"robot_0" : tempDict, "robot_1" : tempDict}
+
+    numWaypoints = len(waypointOffsets)
+    numPositions = len(waypointOffsets) # TODO, add position list to parameters
+
+    for i, offset in enumerate(waypointOffsets):
+        waypointLatLon = meters2lat_lon([apMeters[0] + offset[0], apMeters[1] + offset[1]])
+        waypointLatLonRev = meters2lat_lon([apMeters[0] - offset[0], apMeters[1] - offset[1]])
+        waypointHeading = normalizeAngle(45*i)
+        # waypoints
+        waypointDict["robot_0"]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], waypointAltitude, waypointHeading]})
+        waypointDict["robot_1"]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLonRev[0], waypointLatLonRev[1], waypointAltitude, waypointHeading]})
+        # positions
+        waypointDict["robot_0"]["positions"].update({"position_{}".format(i) : [i, offset[0], offset[1], waypointAltitude, waypointHeading]})
+        waypointDict["robot_1"]["positions"].update({"position_{}".format(i) : [i, -offset[0], -offset[1], waypointAltitude, waypointHeading]})
+
+    # do this betta
+    waypointDict["robot_0"]["numWaypoints"] = numWaypoints
+    waypointDict["robot_0"]["numPositions"] = numPositions
+    waypointDict["robot_1"]["numWaypoints"] = numWaypoints
+    waypointDict["robot_1"]["numPositions"] = numPositions
+
+    rospy.set_param("/robot_list", waypointDict)
+
+if __name__ == "__main__":
+    generateWaypoints()

Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp

 /** @file mission_node.cpp
- *  @version 4.0
- *  @date June 2020
- *
- *  @brief node of hotpoint 1.0/waypoint 1.0.
- *
- *  @Copyright (c) 2020 DJI
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- *
- */
+*  @version 4.0
+*  @date June 2020
+*
+*  @brief node of hotpoint 1.0/waypoint 1.0.
+*
+*  @Copyright (c) 2020 DJI
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy
+* of this software and associated documentation files (the "Software"), to deal
+* in the Software without restriction, including without limitation the rights
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+* copies of the Software, and to permit persons to whom the Software is
+* furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in
+* all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+* SOFTWARE.
+*
+*/
 #include <dji_osdk_ros/bus_driver.h>
 #include <dji_osdk_ros/FlightTaskControl.h>
 #include <dji_osdk_ros/ObtainControlAuthority.h>
 #include <dji_osdk_ros/Overwatch.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Quaternion.h>
+#include <tf2/LinearMath/Quaternion.h>
 #include <string>
 #include <iostream>
 #include <fstream>
@@ -40,94 +44,208 @@ using namespace DJI::OSDK::Telemetry;
 ros::ServiceClient     waypoint_upload_client;
 ros::ServiceClient     waypoint_action_client;
 ros::ServiceClient     flight_control_client;
+ros::ServiceClient     mission_waypoint_client;
 ros::ServiceClient     obtain_ctrl_authority_client;
 ros::Publisher         syncPub; // thing to publish to sync topic "positionSync"
 
-sensor_msgs::NavSatFix gps_pos;
+sensor_msgs::NavSatFix gps_pos; // gps position from drone
+geometry_msgs::Transform ex_pos; // position from external source (qualisys or some such)
 bool                   gps_home_flag = false;
+bool                   pos_home_flag = false;
+bool                   positionMissionFlag = false;
+bool                   waypointMissionFlag = false;
 ros::Subscriber        gps_pos_subscriber;
+ros::Subscriber        pos_feedback_subscriber;
 int                    droneId; // stupid but will work for now
-int                    thresholdCounter = 0; // counts iterations before we check waypoints again
-int                    thresholdLimit = 10; // number of counts before we check ...
+
+int                    waypointMissionUpdateCounter = 0; // counts iterations before we check waypoints again
+int                    waypointMissionUpdateLimit = 10; // number of counts before we check ...
+int                    positionMissionUpdateCounter = 0; // counts iterations before we check positions again
+int                    positionMissionUpdateLimit = 10; // ...
+
 std::vector<WayPointSettings> generatedWaypts; // waypoints to be generated, used to check if we're at waypoint or not
 int                    waypointNumber;
 int                    currentWaypoint = 0; // how many waypoints have we reached
 long double            initialWaypointDistance; // distance to first waypoint
+long double            initialPositionDistance; // distance to first position
 double                 baseSpeedParam;
+double                 minSpeedParam;
+double                 speedScalarParam;
 
+std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
+float poseMagnitudeScaler; // need to run calibration to get
 
 // Utility function for
 // converting degrees to radians
 long double toRadians(const long double degree)
 {
-    // cmath library in C++
-    // defines the constant
-    // M_PI as the value of
-    // pi accurate to 1e-30
-    long double one_deg = (M_PI) / 180;
-    return (one_deg * degree);
+  // cmath library in C++
+  // defines the constant
+  // M_PI as the value of
+  // pi accurate to 1e-30
+  long double one_deg = (M_PI) / 180;
+  return (one_deg * degree);
+}
+
+float eucDistance(std::vector<float> a, std::vector<float> b)
+{
+  // do the math bb
+  return pow(pow(a[0] - b[0],2) + pow(a[1] - b[1],2) + pow(a[2] - b[2],2),0.5);
 }
 
-long double distance(long double lat1, long double long1,
-                     long double lat2, long double long2)
+long double gpsDistance(long double lat1, long double long1, long double lat2, long double long2)
 {
-    // Convert the latitudes
-    // and longitudes
-    // from degree to radians.
-    lat1 = toRadians(lat1);
-    long1 = toRadians(long1);
-    lat2 = toRadians(lat2);
-    long2 = toRadians(long2);
+  // Convert the latitudes
+  // and longitudes
+  // from degree to radians.
+  lat1 = toRadians(lat1);
+  long1 = toRadians(long1);
+  lat2 = toRadians(lat2);
+  long2 = toRadians(long2);
 
-    // Haversine Formula
-    long double dlong = long2 - long1;
-    long double dlat = lat2 - lat1;
+  // Haversine Formula
+  long double dlong = long2 - long1;
+  long double dlat = lat2 - lat1;
 
-    long double ans = pow(sin(dlat / 2), 2) +
-                          cos(lat1) * cos(lat2) *
-                          pow(sin(dlong / 2), 2);
+  long double ans = pow(sin(dlat / 2), 2) +
+  cos(lat1) * cos(lat2) *
+  pow(sin(dlong / 2), 2);
 
-    ans = 2 * asin(sqrt(ans));
+  ans = 2 * asin(sqrt(ans));
 
-    // Radius of Earth in
-    // Kilometers, R = 6371
-    // Use R = 3956 for miles
-    long double R = 6371;
+  // Radius of Earth in
+  // Kilometers, R = 6371
+  // Use R = 3956 for miles
+  long double R = 6371;
 
-    // Calculate the result
-    ans = ans * R;
+  // Calculate the result
+  ans = ans * R;
 
-    // recalculate in meters
-    ans = ans * 1000;
+  // recalculate in meters
+  ans = ans * 1000;
 
-    return ans;
+  return ans;
 }
 
-int countWaypoints()
+std::vector<std::vector<float>> rotateAxisAngle(std::vector<float> u, float angleRad)
 {
-        int waypointCounter = 0;
-        while(ros::param::has("/robot_list/robot_"+to_string(droneId)+"/waypoint_" + to_string(waypointCounter))) // while the next waypoint exists
-        {
-                waypointCounter++;
-        }
-        ROS_INFO("counted %d waypoints in total", waypointCounter);
-        return waypointCounter;
+  const float sinA = sinf( angleRad );
+  const float cosA = cosf( angleRad );
+  const float oneMinusCosA = 1.0f - cosA;
+
+  // std::vector<std::vector<float>> rotationMatrix = (3, 3);
+  // rotationMatrix
+  std::vector<std::vector<float>> rotationMatrix
+  {
+    {(u[0] * u[0] * oneMinusCosA) + cosA, (u[1] * u[0] * oneMinusCosA) - (sinA * u[2]), (u[2] * u[0] * oneMinusCosA) + (sinA * u[1])},
+    {(u[0] * u[1] * oneMinusCosA) + (sinA * u[2]), (u[1] * u[1] * oneMinusCosA) + cosA, (u[2] * u[1] * oneMinusCosA) - (sinA * u[0])},
+    {(u[0] * u[2] * oneMinusCosA) - (sinA * u[1]), (u[1] * u[2] * oneMinusCosA) + (sinA * u[0]), (u[2] * u[2] * oneMinusCosA) + cosA}
+  };
+  return rotationMatrix;
+}
+
+// Function that return
+// dot product of two vector array.
+float dotProduct(std::vector<float> vect_A, std::vector<float> vect_B)
+{
+
+    float product = 0;
+    int vectorSize = vect_A.size(); // both vectors must be of same length
+    // Loop for calculate cot product
+    for (int i = 0; i < vectorSize; i++)
+
+        product = product + vect_A[i] * vect_B[i];
+    return product;
+}
+
+// Function to find
+// cross product of two vector array.
+std::vector<float> crossProduct(std::vector<float> vect_A, std::vector<float> vect_B)
+{
+  std::vector<float> crossP
+  {
+    vect_A[1] * vect_B[2] - vect_A[2] * vect_B[1],
+    vect_A[2] * vect_B[0] - vect_A[0] * vect_B[2],
+    vect_A[0] * vect_B[1] - vect_A[1] * vect_B[0]
+  };
+  return crossP;
+}
+
+std::vector<float> normalizeVector(std::vector<float> vec)
+{
+  float mag = Magnitude3(vec);
+  std::vector<float> result {
+    vec[0]/mag,
+    vec[1]/mag,
+    vec[2]/mag
+  };
+  return result;
+}
+
+float Magnitude3(std::vector<float> v)
+{
+  return pow(pow(v[0], 2) + pow(v[1], 2) + pow(v[2], 2),0.5);
+}
+
+std::vector<float> MatrixMultiply(std::vector <std::vector<float>> a, std::vector<float> b)
+{
+    const int n = a.size();     // a cols
+    const int m = a[0].size();  // a rows
+    std::vector <float> c(n);
+
+    // the PAIN
+    // ROS_INFO("  rotation matrix calc:");
+
+    for (auto k = 0; k < m; ++k) // row loop
+    {
+      for (auto i = 0; i < n; ++i) // col loop
+      {
+        // ROS_INFO("matrix (%d, %d): %f", k, i, a[i][k]);
+        c[k] += a[i][k] * b[i];
+      }
+      // ROS_INFO("result val: %f", c[k]);
+    }
+    return c;
 }
 
+std::vector<std::vector<float>> rotateAlign(std::vector<float> v1, std::vector<float> v2)
+{
+    std::vector<float> axis = normalizeVector(crossProduct(v1, v2));
+    float dotProd = dotProduct(normalizeVector(v1), normalizeVector(v2));
+    if (dotProd < -1.0)
+    {
+      dotProd = -1.0;
+    }
+    else if (dotProd > 1.0)
+    {
+      dotProd = 1.0;
+    }
+    // dotProduct = clamp( dotProduct, -1.0f, 1.0f );
+    float angleRadians = acosf(dotProd);
+    std::vector<std::vector<float>> result = rotateAxisAngle(axis, angleRadians); // returns rotation matrix
+
+    ROS_INFO("axis of rotation: (%f, %f, %f)", axis[0], axis[1], axis[2]);
+    ROS_INFO("dot product: (%f)", dotProd);
+    ROS_INFO("angle rad: (%f)", angleRadians);
+    // ROS_INFO("angle rad: (%f)", angleRadians);
+    return result;
+}
 
 void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
 {
   gps_pos = *msg;
 
+  if (!waypointMissionFlag)
+  {
+    return; // peace out if not running mission
+  }
+
   if (!gps_home_flag)
   {
     int generatedWaypointNumber = static_cast<int>(generatedWaypts.size());
     if (generatedWaypointNumber > 0)
     {
-      initialWaypointDistance = distance(gps_pos.latitude,
-        gps_pos.longitude,
-        generatedWaypts[0].latitude,
-        generatedWaypts[0].longitude);
+      initialWaypointDistance = gpsDistance(gps_pos.latitude, gps_pos.longitude, generatedWaypts[0].latitude, generatedWaypts[0].longitude);
       ROS_INFO("current position: (%f, %f)", static_cast<float32_t>(gps_pos.latitude), static_cast<float32_t>(gps_pos.longitude));
       ROS_INFO("first waypoint position: (%f, %f)", static_cast<float32_t>(generatedWaypts[0].latitude), static_cast<float32_t>(generatedWaypts[0].longitude));
 
@@ -138,10 +256,10 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
   else
   {
     // do checking to see if we're near a waypoint, pause if so
-    if(thresholdCounter >= thresholdLimit)
+    if(waypointMissionUpdateCounter >= waypointMissionUpdateLimit)
     {
 
-      thresholdCounter = 0;
+      waypointMissionUpdateCounter = 0;
       int generatedWaypointNumber = static_cast<int>(generatedWaypts.size());
       if(generatedWaypointNumber > currentWaypoint) // if we actually have waypoints left
       {
@@ -154,49 +272,53 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
         else
         {
           nextWaypointDistance =
-          distance(generatedWaypts[currentWaypoint-1].latitude,
-            generatedWaypts[currentWaypoint-1].longitude,
-            generatedWaypts[currentWaypoint].latitude,
-            generatedWaypts[currentWaypoint].longitude);
+          gpsDistance(generatedWaypts[currentWaypoint-1].latitude, generatedWaypts[currentWaypoint-1].longitude, generatedWaypts[currentWaypoint].latitude, generatedWaypts[currentWaypoint].longitude);
         }
 
-          long double progressDistance =
-          distance(gps_pos.latitude,
-                   gps_pos.longitude,
-                   generatedWaypts[currentWaypoint].latitude,
-                   generatedWaypts[currentWaypoint].longitude);
-          // long double reachedDistance =
-          // distance(generatedWaypts[currentWaypoint].latitude,
-          //          generatedWaypts[currentWaypoint].longitude,
-          //          gps_pos.latitude,
-          //          gps_pos.longitude);
-
-              // now update sync topic with progress through waypoints
-              std_msgs::Float32 syncUpdate;
-              syncUpdate.data = static_cast<float32_t>(currentWaypoint) + static_cast<float32_t>(static_cast<float32_t>(1) - progressDistance/nextWaypointDistance);
-              syncPub.publish(syncUpdate);
-
-              if(progressDistance < tolerance)
-              {
-                currentWaypoint++;
-                ROS_INFO("reached new waypoint! have now reached: \t%d/%d\n", currentWaypoint, generatedWaypointNumber);
-              }
-            }
-          }
-          else
-          {
-            thresholdCounter++;
-          }
-  }
+        long double progressDistance =
+        gpsDistance(gps_pos.latitude, gps_pos.longitude, generatedWaypts[currentWaypoint].latitude, generatedWaypts[currentWaypoint].longitude);
 
+        // now update sync topic with progress through waypoints
+        std_msgs::Float32 syncUpdate;
+        syncUpdate.data = static_cast<float32_t>(currentWaypoint) + static_cast<float32_t>(static_cast<float32_t>(1) - progressDistance/nextWaypointDistance);
+        syncPub.publish(syncUpdate);
 
+        if(progressDistance < tolerance)
+        {
+          currentWaypoint++;
+          ROS_INFO("reached new waypoint! have now reached: \t%d/%d\n", currentWaypoint, generatedWaypointNumber);
+        }
+      }
+    }
+    else
+    {
+      waypointMissionUpdateCounter++;
+    }
+  }
 }
 
+void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
+{
+  ex_pos = *msg; // alllllways load the message
+
+  if (!positionMissionFlag)
+  {
+    return; // peace out if not running mission
+  }
+
+  // do loopin' stuff to get close to waypoints
+}
 
 bool setupWaypointMission(int responseTimeout)
 {
   ros::spinOnce();
 
+  // ROS_INFO();
+  while(!ros::param::has("/robot_list/robot_"+to_string(droneId)+"/numWaypoints"))
+  {
+    ROS_INFO("waiting for waypoints to load...");
+  }
+
   // Waypoint Mission : Initialization
   dji_osdk_ros::MissionWaypointTask waypointTask;
   setWaypointInitDefaults(waypointTask);
@@ -225,12 +347,13 @@ bool setupWaypointMission(int responseTimeout)
   return true;
 }
 
+// global waypoint mission
+
 bool runWaypointMission()
 {
 
   // Waypoint Mission: Start
-  if (missionAction(DJI_MISSION_TYPE::WAYPOINT,
-                    MISSION_ACTION::START).result)
+  if (missionAction(DJI_MISSION_TYPE::WAYPOINT, MISSION_ACTION::START).result)
   {
     ROS_INFO("Mission start command sent successfully");
   }
@@ -240,6 +363,83 @@ bool runWaypointMission()
     return false;
   }
 
+  waypointMissionFlag = true;
+  positionMissionFlag = false;
+  return true;
+}
+
+// relative position mission
+
+bool runPositionMission()
+{
+
+
+  int numPositions;
+  ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions);
+  int currentPositionIndex = 0; // will increment
+  ros::Rate updateRate(20); // update period for topics/commands
+  positionMissionFlag = true;
+
+  while (positionMissionFlag && ros::ok())
+  {
+    // print distance to first position
+    std::vector<float> currentPos
+    {
+      static_cast<float>(ex_pos.translation.x),
+      static_cast<float>(ex_pos.translation.y),
+      static_cast<float>(ex_pos.translation.z)
+    };
+    std::vector<float> tempPos;
+    ros::param::get("/robot_list/robot_" + to_string(droneId) + "/positions/position_" + to_string(currentPositionIndex), tempPos);
+    // pull out x,y,z
+    std::vector<float> targetPos
+    {
+      tempPos[1], // 0th index is the index of position, facepalm
+      tempPos[2],
+      tempPos[3]
+    };
+    // ROS_INFO("current position: (%f %f %f)\n",currentPos[0],currentPos[1],currentPos[2]);
+    // ROS_INFO("target position: (%f %f %f)\n",targetPos[0],targetPos[1],targetPos[2]);
+    // map to our coordinate system
+    if (!ros::param::has("poseRotationMatrix/"))
+    {
+      // run rotation matrix calc
+      calibratePositionReference();
+    }
+
+    // convert both target and current position, and move between
+    std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
+    std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
+
+    std::vector<float> moveCommand
+    {
+      targetPosMapped[0] - currentPosMapped[0],
+      targetPosMapped[1] - currentPosMapped[1],
+      targetPosMapped[2] - currentPosMapped[2]
+    };
+
+    // go to position
+    relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], 0.0);
+
+    ros::spinOnce();
+
+    // check if we're close to position
+    float checkDist = eucDistance(currentPos, targetPos);
+    if (checkDist <= 0.2) // close ish
+    {
+      currentPositionIndex += 1; // go to next position
+    }
+
+    if (currentPositionIndex >= numPositions)
+    {
+      // leave
+      positionMissionFlag = false;
+    }
+    // ROS_INFO("distance to first position: %f", firstPosDist);
+
+
+    updateRate.sleep();
+  }
   return true;
 }
 
@@ -266,70 +466,60 @@ void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask)
   waypointTask.idle_velocity      = baseSpeedParam;
   waypointTask.action_on_finish   = dji_osdk_ros::MissionWaypointTask::FINISH_NO_ACTION;
   waypointTask.mission_exec_times = 1;
-  waypointTask.yaw_mode		  = dji_osdk_ros::MissionWaypointTask::YAW_MODE_WAYPOINT;
+  waypointTask.yaw_mode		  = dji_osdk_ros::MissionWaypointTask::YAW_MODE_RC;
   waypointTask.trace_mode	  = dji_osdk_ros::MissionWaypointTask::TRACE_COORDINATED;
   waypointTask.action_on_rc_lost  = dji_osdk_ros::MissionWaypointTask::ACTION_AUTO;
   waypointTask.gimbal_pitch_mode  = dji_osdk_ros::MissionWaypointTask::GIMBAL_PITCH_FREE;
 }
 
-// std::vector<DJI::OSDK::WayPointSettings> createWaypoints()
-// {
-//
-//   std::vector<DJI::OSDK::WayPointSettings> wpVector =
-//     loadWaypoints();
-//   return wpVector;
-//
-// }
-
 std::vector<DJI::OSDK::WayPointSettings> loadWaypoints()
 {
 
   std::vector<double> wpt; // single waypoint
   // Let's create a vector to store our waypoints in.
   std::vector<DJI::OSDK::WayPointSettings> wp_list;
-  // start_data->index = 0;
-  // wp_list.push_back(*start_data);
-  int i = 0;
-  while(ros::param::has("/robot_list/robot_"+to_string(droneId)+"/waypoint_" + to_string(i))) // while the next waypoint exists
-  {
-    ros::param::get("/robot_list/robot_"+to_string(droneId)+"/waypoint_" + to_string(i), wpt);
-    i++; // iterate to next parameter
-    ROS_INFO("Waypoint read as (Ind, Lat, Lon, Alt, Yaw): %f %f %f %f %f\n ", wpt[0], wpt[1], wpt[2], wpt[3], wpt[4]);
+  int numPts;
+  ros::param::get("/robot_list/robot_"+to_string(droneId)+"/numWaypoints", numPts);
+  for (int i = 0; i < numPts; i++) {
+    ros::param::get("/robot_list/robot_"+to_string(droneId)+"/waypoints/waypoint_" + to_string(i), wpt);
+    ROS_INFO("Waypoint read as (Ind, Lat, Lon, Alt, Yaw): %f %f %f %f %d\n ", wpt[0], wpt[1], wpt[2], wpt[3], wpt[4]);
     WayPointSettings wp;
     setWaypointDefaults(&wp);
     wp.index     = i;
     wp.latitude = wpt[1];
     wp.longitude = wpt[2];
     wp.altitude = wpt[3];
-    // wp.yaw = static_cast<int>(wpt[4]);
-    wp.yaw = 0;
+    wp.yaw = wpt[4];
     wp_list.push_back(wp);
   }
-
   return wp_list;
 
 }
 
-void uploadWaypoints(std::vector<DJI::OSDK::WayPointSettings>& wp_list,
-                     int responseTimeout,
-                     dji_osdk_ros::MissionWaypointTask& waypointTask)
+void uploadWaypoints(std::vector<DJI::OSDK::WayPointSettings>& wp_list, int responseTimeout, dji_osdk_ros::MissionWaypointTask& waypointTask)
 {
   dji_osdk_ros::MissionWaypoint waypoint;
   double target_yaw = 0.0;
-  for (std::vector<WayPointSettings>::iterator wp = wp_list.begin();
-       wp != wp_list.end(); ++wp)
-  {
+  // for (std::vector<WayPointSettings>::iterator wp = wp_list.begin();
+  //      wp != wp_list.end(); ++wp)
+  // {
+  int numPts;
+  DJI::OSDK::WayPointSettings wp;
+  ros::param::get("/robot_list/robot_"+to_string(droneId)+"/numWaypoints", numPts);
+  for (int i = 0; i < numPts; i++) {
+
+    wp = wp_list[i];
     // target_yaw += 15.0;
-    waypoint.latitude            = wp->latitude;
-    waypoint.longitude           = wp->longitude;
-    waypoint.altitude            = wp->altitude;
-    waypoint.damping_distance    = wp->damping;
-    waypoint.target_yaw          = wp->yaw;
+    waypoint.latitude            = wp.latitude;
+    waypoint.longitude           = wp.longitude;
+    waypoint.altitude            = wp.altitude;
+    waypoint.damping_distance    = wp.damping;
+    waypoint.target_yaw          = wp.yaw;
     waypoint.target_gimbal_pitch = 0;
-    waypoint.turn_mode           = wp->turnMode;
+    waypoint.turn_mode           = wp.turnMode;
     waypoint.has_action          = 0;
     waypointTask.mission_waypoint.push_back(waypoint);
-    ROS_INFO("Waypoint created at (Lat, Lon, Alt, Dmp, Yaw): %f %f %f %f %f\n ", waypoint.latitude, waypoint.longitude, waypoint.altitude, waypoint.damping_distance, waypoint.target_yaw);
+    ROS_INFO("Waypoint created at (Lat, Lon, Alt, Dmp, Yaw): %f %f %f %f %d\n ", waypoint.latitude, waypoint.longitude, waypoint.altitude, waypoint.damping_distance, waypoint.target_yaw);
   }
 }
 
@@ -341,7 +531,7 @@ ServiceAck initWaypointMission(dji_osdk_ros::MissionWaypointTask& waypointTask)
   if (!missionWpUpload.response.result)
   {
     ROS_WARN("ack.info: set = %i id = %i", missionWpUpload.response.cmd_set,
-             missionWpUpload.response.cmd_id);
+    missionWpUpload.response.cmd_id);
     ROS_WARN("ack.data: %i", missionWpUpload.response.ack_data);
   }
   return ServiceAck(
@@ -350,34 +540,32 @@ ServiceAck initWaypointMission(dji_osdk_ros::MissionWaypointTask& waypointTask)
 }
 
 ServiceAck missionAction(DJI::OSDK::DJI_MISSION_TYPE type,
-              DJI::OSDK::MISSION_ACTION   action)
+  DJI::OSDK::MISSION_ACTION   action)
 {
-
   dji_osdk_ros::MissionWpAction missionWpAction;
   dji_osdk_ros::MissionHpAction missionHpAction;
   switch (type)
   {
     case DJI::OSDK::WAYPOINT:
-      missionWpAction.request.action = action;
-      waypoint_action_client.call(missionWpAction);
-      if (!missionWpAction.response.result)
-      {
-        ROS_WARN("ack.info: set = %i id = %i", missionWpAction.response.cmd_set,
-                 missionWpAction.response.cmd_id);
-        ROS_WARN("ack.data: %i", missionWpAction.response.ack_data);
-      }
-      return { missionWpAction.response.result,
-               missionWpAction.response.cmd_set,
-               missionWpAction.response.cmd_id,
-               missionWpAction.response.ack_data };
+    missionWpAction.request.action = action;
+    waypoint_action_client.call(missionWpAction);
+    if (!missionWpAction.response.result)
+    {
+      ROS_WARN("ack.info: set = %i id = %i", missionWpAction.response.cmd_set,
+      missionWpAction.response.cmd_id);
+      ROS_WARN("ack.data: %i", missionWpAction.response.ack_data);
+    }
+    return { missionWpAction.response.result,
+      missionWpAction.response.cmd_set,
+      missionWpAction.response.cmd_id,
+      missionWpAction.response.ack_data };
   }
-
 }
 
 bool takeoff()
 {
   dji_osdk_ros::FlightTaskControl flightTaskControl;
-  flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request ::TASK_TAKEOFF;
+  flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_TAKEOFF;
   flight_control_client.call(flightTaskControl);
 
   return flightTaskControl.response.result;
@@ -386,24 +574,193 @@ bool takeoff()
 bool land()
 {
   dji_osdk_ros::FlightTaskControl flightTaskControl;
-  flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_LAND;
+  flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_FORCE_LANDING;
   flight_control_client.call(flightTaskControl);
 
   return flightTaskControl.response.result;
 }
 
+bool relativePosition(float x, float y, float z, float yaw)
+{
+  // convert relative position to velocity command
+  std::vector<float> posVec {x, y, z};
+  float totalDist = Magnitude3(posVec);
+  ROS_INFO("uncapped speed: %f", speedScalarParam*pow(totalDist,2) + minSpeedParam);
+  ROS_INFO("base speed: %f", baseSpeedParam);
+  ROS_INFO("min speed: %f", minSpeedParam);
+  ROS_INFO("speed scalar: %f", speedScalarParam);
+
+  float speed = min(speedScalarParam*pow(totalDist,2) + minSpeedParam, baseSpeedParam);
+
+  float totalTimeMs = (totalDist/speed)*1000.0; // convert from s to ms
+  std::vector<float> velDir = normalizeVector(posVec);
+  std::vector<float> velVec = {velDir[0]*speed, velDir[1]*speed, velDir[2]*speed};
+
+  ROS_INFO("velocity vector: (%f, %f, %f)", velVec[0], velVec[1], velVec[2]);
+  ROS_INFO("speed value: %f", speed);
+
+  ROS_INFO("distance: %f", totalDist);
+  ROS_INFO("time: %f", totalTimeMs);
+
+  dji_osdk_ros::FlightTaskControl flightTaskControl;
+  dji_osdk_ros::JoystickParams joystickParams;
+  joystickParams.x = velVec[0];
+  joystickParams.y = velVec[1];
+  joystickParams.z = velVec[2];
+  joystickParams.yaw = yaw;
+  flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL;
+  flightTaskControl.request.joystickCommand = joystickParams;
+  flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
+  flightTaskControl.request.posThresholdInM = 0.1;
+  flightTaskControl.request.yawThresholdInDeg = 1;
+
+  flight_control_client.call(flightTaskControl); // actually waits for drone to "reach" position
+
+  return flightTaskControl.response.result;
+}
+
+bool stopMotion()
+{
+  if (waypointMissionFlag)
+  {
+    dji_osdk_ros::MissionWpAction waypointAction;
+    waypointAction.request.action = dji_osdk_ros::MissionWpAction::Request::ACTION_STOP; // stahhp
+    waypoint_action_client.call(waypointAction);
+    waypointMissionFlag = false;
+  }
+
+  if (positionMissionFlag)
+  {
+    positionMissionFlag = false;
+  }
+  return true;
+}
+
+bool goHome()
+{
+  // TODO fix this pls
+  return true;
+}
+
+void calibratePositionReference()
+{
+  // call move and check what pos ref says
+  ros::spinOnce(); // let callbacks update
+  geometry_msgs::Transform preT = ex_pos;
+  relativePosition(0.5, 0.0, 0.0, 0.0);
+  ros::spinOnce(); // let callbacks update
+  geometry_msgs::Transform postT = ex_pos;
+
+  std::vector<float> requestedPosition {0.5, 0.0, 0.0}; // what we asked for
+  std::vector<float> reversePosition {-0.5, 0.0, 0.0}; // what we asked for
+  std::vector<float> actualPosition // what we got
+  {
+    static_cast<float>(postT.translation.x) - static_cast<float>(preT.translation.x),
+    static_cast<float>(postT.translation.y) - static_cast<float>(preT.translation.y),
+    static_cast<float>(postT.translation.z) - static_cast<float>(preT.translation.z),
+  };
+
+  // calculate rotation matrix
+  poseRotationMatrix = rotateAlign(requestedPosition, actualPosition);
+
+  // now get scaling difference between coordinate frames
+  float requestedPositionMag = Magnitude3(requestedPosition);
+  float actualPositionMag = Magnitude3(actualPosition);
+
+  poseMagnitudeScaler = requestedPositionMag/actualPositionMag;
+
+  ROS_INFO("pose magnitude scalar %f", poseMagnitudeScaler);
+
+  std::vector<float> checkPosition = applyRotationMatrixScaling(requestedPosition);
+  relativePosition(reversePosition[0], reversePosition[1], reversePosition[2], 0.0); // go to requested position in corrected frame
+  ros::spinOnce();
+
+  // write rotation and scale to parameters
+  for (int i = 0; i < poseRotationMatrix.size(); i++) {
+    for (int j = 0; j < poseRotationMatrix[0].size(); j++) {
+      ros::param::set("poseRotationMatrix/val_" + to_string(i) + to_string(j), poseRotationMatrix[i][j]);
+    }
+  }
+  ros::param::set("poseMagnitudeScaler", poseMagnitudeScaler);
+}
+
+std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec)
+{
+  std::vector<float> rotatedVec = MatrixMultiply(poseRotationMatrix, inputVec);
+  std::vector<float> outputVec
+  {
+    rotatedVec[0]*poseMagnitudeScaler,
+    rotatedVec[1]*poseMagnitudeScaler,
+    rotatedVec[2]*poseMagnitudeScaler
+  };
+  return outputVec;
+}
+
 // bool add(beginner_tutorials::AddTwoInts::Request &req, beginner_tutorials::AddTwoInts::Response &res)
 bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res)
 {
-  ROS_INFO("takeoff command received");
-  // ros::Duration(7.5).sleep();
 
+  // always request control?
   dji_osdk_ros::ObtainControlAuthority obtainCtrlAuthority;
   obtainCtrlAuthority.request.enable_obtain = true;
   obtain_ctrl_authority_client.call(obtainCtrlAuthority);
 
-  // count number of waypoints
-  runWaypointMission();
+  // switch based on requested action
+  switch (req.action) {
+    case 1:
+    {
+      ROS_INFO("received overwatch: start waypoint");
+      // start waypoint
+      int responseTimeout = 1;
+      setupWaypointMission(responseTimeout); // for now!
+      runWaypointMission();
+      break;
+    }
+    case 2:
+    {
+      ROS_INFO("received overwatch: start position");
+      // start position
+      runPositionMission();
+      break;
+    }
+    case 3:
+    {
+      ROS_INFO("received overwatch: stop all motion");
+      // stop all motion
+      stopMotion();
+      break;
+    }
+    case 4:
+    {
+      ROS_INFO("received overwatch: land");
+      // land
+      land();
+      break;
+    }
+    case 5:
+    {
+      ROS_INFO("received overwatch: go home");
+      // go home
+      goHome();
+      break;
+    }
+    case 6:
+    {
+      ROS_INFO("received overwatch: takeoff");
+      // go home
+      takeoff();
+      break;
+    }
+    case 7:
+    {
+      ROS_INFO("received overwatch: calibrate pos ref");
+      // go home
+      calibratePositionReference();
+      break;
+    }
+    default:
+      break;
+  }
 
   res.result = true;
   return res.result;
@@ -416,6 +773,8 @@ int main(int argc, char** argv)
   ros::NodeHandle nh;
 
   ros::param::get("base_speed", baseSpeedParam);
+  ros::param::get("min_speed", minSpeedParam);
+  ros::param::get("speed_scalar", speedScalarParam);
   ros::param::get("drone_id", droneId);
   ROS_INFO("base speed set to %f", baseSpeedParam);
 
@@ -423,30 +782,28 @@ int main(int argc, char** argv)
   // obtain control authority?
   obtain_ctrl_authority_client = nh.serviceClient<dji_osdk_ros::ObtainControlAuthority>(
     "obtain_release_control_authority");
+
   waypoint_upload_client = nh.serviceClient<dji_osdk_ros::MissionWpUpload>(
     "dji_osdk_ros/mission_waypoint_upload");
+
   waypoint_action_client = nh.serviceClient<dji_osdk_ros::MissionWpAction>(
     "dji_osdk_ros/mission_waypoint_action");
+
   flight_control_client = nh.serviceClient<dji_osdk_ros::FlightTaskControl>(
     "flight_task_control");
-  syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 10);
-  gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>(
-    "dji_osdk_ros/gps_position", 10, &gpsPosCallback);
 
+  syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1);
+  gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>("dji_osdk_ros/gps_position", 1, &gpsPosCallback);
+  pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("pose_feedback", 1, &posFeedbackCallback);
 
-  ros::ServiceServer overwatchService = nh.advertiseService("Overwatch", overwatchFunction);
+  ros::ServiceServer overwatchService = nh.advertiseService("overwatch", overwatchFunction);
 
-  // wait for waypoints to be written
-  // ros::Duration(6.0).sleep();
   // wait for services to be up
   obtain_ctrl_authority_client.waitForExistence();
   waypoint_upload_client.waitForExistence();
   waypoint_action_client.waitForExistence();
   flight_control_client.waitForExistence();
-
-  int responseTimeout = 1;
-  setupWaypointMission(responseTimeout);
-
+  // wait for waypoints to be up as well
   ROS_INFO("bus driver ready to go!");
 
   ros::spin();

Onboard-SDK-ROS/srv/Overwatch.srv

+# constants for different actions
+uint8 START_WAYPOINT_PATH = 1
+uint8 START_POSITION_PATH = 2
+uint8 STOP_MOTION = 3
+uint8 LAND = 4
+uint8 GO_HOME = 5
+uint8 TAKEOFF = 6
+uint8 CALIBRATE_POS_REF = 7
+
+# request
 bool start
+uint8 action
 ---
+# response
 bool result

database_tools/scripts/db_stream.py

-#!/usr/bin/python3
-import rospy
-import numpy as np
-from sensor_msgs.msg import NavSatFix
-
-def talker():
-    pub = rospy.Publisher('/dji_osdk_ros/gps_position', NavSatFix, queue_size=10)
-    rospy.init_node('db_streamer', anonymous=True)
-    rate = rospy.Rate(3) # 3hz
-    navData = NavSatFix()
-    navData.header.frame_id = "NaN"
-    while not rospy.is_shutdown():
-        navData.latitude = -80.564890 + 0.1*(np.random.rand()) # give it jiggle
-        navData.longitude = 37.199879 + 0.1*(np.random.rand())
-        navData.altitude = 30
-        pub.publish(navData)
-        rate.sleep()
-
-if __name__ == '__main__':
-    try:
-        talker()
-    except rospy.ROSInterruptException:
-        pass
-

database_tools/scripts/db_upload.py

-#!/usr/bin/python3
-import rospy
-from functools import partial
-import mysql.connector
-import datetime
-import json
-from std_msgs.msg import String
-from sensor_msgs.msg import NavSatFix
-
-def shutdown_handler(args):
-    db_conn = args[0]
-    db_cursor = args[1]
-    db_conn.close()
-    db_cursor.close()
-    rospy.loginfo("shuttin' down, bye")
-    print("shuttin' down, bye")
-
-def callback(data, args):
-  mydb = args[0]
-  mycursor = args[1]
-  id_num = args[2]
-  table = args[3]
-
-  #rospy.loginfo(rospy.get_caller_id() + "%s", data)
-  #rospy.loginfo("connection status: " + str(mydb.is_connected()))
-  if data.header.seq % 30 == 0: # only update every 10th message to avoid over crowding the sql
-    gps_data = {
-      "gps" : [
-        {
-          "stamp" : data.header.seq,
-          "timestamp" : data.header.stamp.secs,
-          "lat" : data.latitude,
-          "long" : data.longitude
-        },
-      ]
-    }
-    sql_data = {'deviceid' : 'drone_0',
-          'taskid':'search_0',
-          'gpsdata': json.dumps(gps_data),
-          'created_at':datetime.datetime.now(),
-          'updated_at':datetime.datetime.now()}
-
-    sql_update = 'UPDATE ' + table + \
-    ' SET {}'.format(', '.join('{}=%s'.format(k) for k in sql_data)) +\
-    ' WHERE deviceid = %s'
-    result = mycursor.execute(sql_update, list(sql_data.values())+['drone_0'])
-    result = mydb.commit()
-    rospy.loginfo("GUI gps position updated, lat: {}, lon: {}".format(data.latitude, data.longitude))
-
-def listener():
-
-    # In ROS, nodes are uniquely named. If two nodes with the same
-    # name are launched, the previous one is kicked off. The
-    # anonymous=True flag means that rospy will choose a unique
-    # name for our 'listener' node so that multiple listeners can
-    # run simultaneously.
-
-    table = 'app3_gpsdata'
-
-    mydb = mysql.connector.connect(
-      host="192.168.1.100",
-      #user="larkin_h",
-      #passwd="Meepp973",
-      database="/home/llh/sarwebui/db.sqlite3",
-      #auth_plugin="mysql_native_password"
-    )
-
-    mycursor = mydb.cursor()
-
-    id_num = 4 # id which we want to update
-
-    rospy.init_node('db_uploader', anonymous=True)
-    rospy.Subscriber("dji_osdk_ros/gps_position", NavSatFix, callback, (mydb, mycursor, id_num, table))
-
-    # upload waypoints to GUI
-    waypoint_dict = rospy.get_param("/robot_list/robot_0")
-    waypoint_json = []
-    for i,pt in enumerate(waypoint_dict.keys()):
-        waypoint_json.append({'stamp' : waypoint_dict[pt][0], 'timestamp' : -1, 'lat' : waypoint_dict[pt][2], 'long' : waypoint_dict[pt][3]}) # leaving out altitude for now!
-    
-    waypoint_json = sorted(waypoint_json, key = lambda i: i['stamp'])
-    wp_table = 'app3_waypointsdata'
-    wp_data = {'deviceid' : 'drone_0',
-               'taskid' : 'search_0',
-               'waypointsdata' : json.dumps(waypoint_json),
-               'created_at':datetime.datetime.now(),
-               'updated_at':datetime.datetime.now()}
-
-    sql_waypoint_update = 'UPDATE ' + wp_table + ' SET {}'.format(', '.join('{}=%s'.format(k) for k in wp_data)) + ' WHERE deviceid = %s'
-    mycursor.execute(sql_waypoint_update, list(wp_data.values()) + ['drone_0'])
-    mydb.commit()
-    rospy.loginfo("waypoints updated")
-
-    rospy.on_shutdown(partial(shutdown_handler, (mydb, mycursor)))
-    # spin() simply keeps python from exiting until this node is stopped
-    rospy.spin()
-
-if __name__ == '__main__':
-    listener()
-