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/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()
-