added better waypoints and mods

Onboard-SDK-ROS/launch/local_test.launch

 <launch>
-    <rosparam file="$(find dji_osdk_ros)/launch/waypoints_musical_drones.json"/>
+    <!-- <rosparam file="$(find dji_osdk_ros)/launch/waypoints_musical_drones.json"/> -->
     <node pkg="dji_osdk_ros" type="dji_vehicle_node" name="vehicle_node" output="screen">
     <!-- node parameters -->
     <param name="acm_name" type="string" value="/dev/ttyACM0"/>
@@ -26,12 +26,9 @@
     </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>
+      <rosparam command="load" param="anchorPoint"> [37.196986, -80.578257] </rosparam>
+      <rosparam command="load" param="altitude"> 2.0 </rosparam>
+      <rosparam command="load" param="droneNum"> 3 </rosparam>
     </node>
 
     <!-- <node pkg="dji_osdk_ros" type="image_saver.py" name="image_saver" output="screen"/> -->

Onboard-SDK-ROS/scripts/poseFeedbackConversion.py

@@ -4,6 +4,7 @@ import rospy
 import numpy as np
 from sensor_msgs.msg import NavSatFix
 from geometry_msgs.msg import PointStamped
+from geometry_msgs.msg import QuaternionStamped
 from dji_osdk_ros.msg import VOPosition
 from geometry_msgs.msg import Transform
 from geometry_msgs.msg import Vector3
@@ -16,6 +17,7 @@ homeSetFlag = False
 # homePoint = PointStamped()
 homePoint = NavSatFix()
 transform = Transform()
+quat = Quaternion()
 angleOffset = 0.0
 
 def point_rotation(origin, pt, ang):
@@ -30,19 +32,17 @@ def point_rotation(origin, pt, ang):
     return pt_spun
 
 
+def rotCallback(data):
+    global quat
+    quat = data.quaternion
 
-def callback(data):
+def posCallback(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...")
 
@@ -71,20 +71,21 @@ def callback(data):
     translation.z = data.altitude - homePoint.altitude
 
     transform.translation = translation
+    transform.rotation = quat
 
-    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]
+    # 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)
+    rospy.Subscriber("dji_osdk_ros/gps_position", NavSatFix, posCallback, queue_size = 1)
+    rospy.Subscriber("dji_osdk_ros/attitude", QuaternionStamped, rotCallback, queue_size = 1)
     posePublisher = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
     while not rospy.is_shutdown():
         posePublisher.publish(transform)

Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py

@@ -3,7 +3,6 @@ from tools import meters2lat_lon, lat_lon2meters
 import numpy as np
 import rospy
 
-
 def normalizeAngle(angle):
     newAngle = angle%360
     newAngle = (newAngle + 360)%360
@@ -12,11 +11,14 @@ def normalizeAngle(angle):
     return newAngle
 
 def generateWaypoints():
-
     rospy.init_node("waypointGenerator")
     anchorPoint = rospy.get_param("~anchorPoint")
-    waypointAltitude = rospy.get_param("~altitude")
+    baseWaypointAltitude = rospy.get_param("~altitude")
+    droneNum = rospy.get_param("~droneNum")
 
+    waypointAltitudeIncrement = baseWaypointAltitude # increase altitude for each drone in team
+
+    # musical drones trajectory is convient test case, but general path is possible
     # ring sizes
     big = 10
     med = 5
@@ -37,29 +39,38 @@ def generateWaypoints():
     [big, -big],
     ]
     tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0}
-    waypointDict = {"robot_0" : tempDict, "robot_1" : tempDict}
-
+    waypointDict = {}
+    # define dictionary
+    # for temp in range(droneNum):
+    #     waypointDict["robot_{}".format(temp)] = tempDict
     numWaypoints = len(waypointOffsets)
     numPositions = len(waypointOffsets) # TODO, add position list to parameters
 
+    # print(waypointDict)
+
+    for d in range(droneNum):
+        # rospy.loginfo("blah!")
+        # print(d)
+        waypointDict["robot_{}".format(d)] = {}
+        waypointDict["robot_{}".format(d)] = {}
+        waypointDict["robot_{}".format(d)]["waypoints"] = {}
+        waypointDict["robot_{}".format(d)]["positions"] = {}
         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]})
+            waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseWaypointAltitude + d*waypointAltitudeIncrement, 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]})
+            waypointDict["robot_{}".format(d)]["positions"].update({"position_{}".format(i) : [i, offset[0], offset[1], baseWaypointAltitude + d*waypointAltitudeIncrement, waypointHeading]})
 
         # do this betta
-    waypointDict["robot_0"]["numWaypoints"] = numWaypoints
-    waypointDict["robot_0"]["numPositions"] = numPositions
-    waypointDict["robot_1"]["numWaypoints"] = numWaypoints
-    waypointDict["robot_1"]["numPositions"] = numPositions
-
+        waypointDict["robot_{}".format(d)]["numWaypoints"] = numWaypoints
+        waypointDict["robot_{}".format(d)]["numPositions"] = numPositions
+    # print(waypointDict)
     rospy.set_param("/robot_list", waypointDict)
+    # print(waypointDict)
 
 if __name__ == "__main__":
     generateWaypoints()

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

@@ -37,6 +37,9 @@
 #include <iostream>
 #include <fstream>
 
+#include <Eigen3/Eigen/Geometry>
+#include <Eigen3/Eigen/Dense>
+
 using namespace DJI::OSDK;
 using namespace DJI::OSDK::Telemetry;
 
@@ -372,7 +375,6 @@ bool runWaypointMission()
 bool runPositionMission()
 {
 
-
   int numPositions;
   ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions);
   int currentPositionIndex = 0; // will increment
@@ -397,8 +399,16 @@ bool runPositionMission()
       tempPos[2],
       tempPos[3]
     };
-    // float targetYaw = tempPos[4]; // TODO: need yaw measurement for this to work1!!!!1!
-    // float currentYaw = ex_pos.rotation.
+
+    // may need to rotate this yaw to match coordinate system
+    float targetYaw = (M_PI/180.0)*tempPos[4];
+    Eigen::Quaternion<float> currentRotation = {float(ex_pos.rotation.w), float(ex_pos.rotation.x), float(ex_pos.rotation.y), float(ex_pos.rotation.z)};
+    auto currentRotationEuler = currentRotation.toRotationMatrix().eulerAngles(0, 1, 2);
+    // TODO need angle mapping right here!!!!
+    float currentYaw = currentRotationEuler[2];
+    ROS_INFO("target yaw: %f", targetYaw);
+    ROS_INFO("current yaw: %f", currentYaw);
+    float yawCommand = (180.0/M_PI)*(targetYaw - currentYaw);
     // map to our coordinate system
 
     if (!rotationMatrixCalibratedFlag) // could also do this before loop?
@@ -422,7 +432,7 @@ bool runPositionMission()
     };
 
     // go to position
-    relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], 0.0);
+    relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
 
     ros::spinOnce();
 
@@ -601,29 +611,18 @@ 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, aim just short of position
   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; // for now until i have yaw measurement up
+  joystickParams.yaw = yaw/(totalTimeMs/1000.0); // assuming this is in rads/sec??
   flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL;
   flightTaskControl.request.joystickCommand = joystickParams;
   flightTaskControl.request.velocityControlTimeMs = totalTimeMs;