qualisys further changes

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

@@ -71,6 +71,7 @@ typedef struct ServiceAck
 
 
 float eucDistance(std::vector<float> a, std::vector<float> b);
+float normalizeAngle(float angle);
 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);

Onboard-SDK-ROS/launch/local_test.launch

@@ -11,18 +11,27 @@
     <param name="enc_key" type="string" value="e180b993ca8365653437fbafe7211ba040386d77c3c87627882857a11bd8efbd"/>
     <param name="use_broadcast" type="bool" value="false"/>
     </node>
+    <!-- <param name="min_yaw_speed" type="double" value="0.0"/>
+    <param name="max_yaw_speed" type="double" value="15.0"/> -->
+
     <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"/>
+    <!-- max yaw rate in deg/s -->
+    <param name="max_yaw_speed" type="double" value="30.0"/>
+    <param name="position_playback" type="bool" value="false"/>
     <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">
+    <!-- <node pkg="dji_osdk_ros" type="poseFeedbackConversion.py" name="poseFeedbackConversion" output="screen">
       <rosparam command="load" param="angleOffset">
-        -33.0
+        0.0
       </rosparam>
+    </node> -->
+    <node pkg="dji_osdk_ros" type="qualisysFeedback.py" name="qualisysFeedback" output="screen">
+      <param name="trackName" type="string" value="M100_CarbonFiber"/>
     </node>
 
     <node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros">

Onboard-SDK-ROS/scripts/poseFeedbackConversion.py

@@ -3,6 +3,7 @@
 import rospy
 import numpy as np
 from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import Imu
 from geometry_msgs.msg import PointStamped
 from geometry_msgs.msg import QuaternionStamped
 from dji_osdk_ros.msg import VOPosition
@@ -34,7 +35,9 @@ def point_rotation(origin, pt, ang):
 
 def rotCallback(data):
     global quat
-    quat = data.quaternion
+    quat = data.orientation;
+    # # FINE ill DO it MYself all ALONE
+
 
 def posCallback(data):
     global homeSetFlag
@@ -51,21 +54,16 @@ def posCallback(data):
     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)
 
+
+    # performing dark magics
     translation.x = currentRotatedOffset[1]
     translation.y = currentRotatedOffset[0]
     translation.z = data.altitude - homePoint.altitude
@@ -73,19 +71,11 @@ def posCallback(data):
     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]
-
-
 def main():
     global transform
     rospy.init_node("poseFeedbackConversion")
     rospy.Subscriber("dji_osdk_ros/gps_position", NavSatFix, posCallback, queue_size = 1)
-    rospy.Subscriber("dji_osdk_ros/attitude", QuaternionStamped, rotCallback, queue_size = 1)
+    rospy.Subscriber("dji_osdk_ros/imu", Imu, 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/qualisysFeedback.py

+#!/usr/bin/python3
+
+import rospy
+import numpy as np
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Transform
+
+transform = Transform()
+def callback(data):
+    # save the pose date in transform
+    transform.rotation = data.pose.orientation
+    # we must remapp
+    # transform.translation = data.pose.position
+    transform.translation.x = data.pose.position.z
+    transform.translation.z = data.pose.position.y
+    transform.translation.y = data.pose.position.x
+
+def main():
+    global transform
+    rospy.init_node("qualisysFeedback")
+    name = rospy.get_param("~trackName")
+    rospy.loginfo("looking for track of name " + str(name) + " ...")
+    rospy.Subscriber("/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
+    pub = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
+    while not rospy.is_shutdown():
+        pub.publish(transform)
+
+
+if __name__ == '__main__':
+    main()

Onboard-SDK-ROS/scripts/tools.py

@@ -12,3 +12,10 @@ def lat_lon2meters(latlon_point):
     transformer = Transformer.from_crs(4326, 3857)
     meters_point = transformer.transform(latlon_point[0], latlon_point[1])
     return meters_point
+
+def normalizeAngle(angle):
+    newAngle = angle%360
+    newAngle = (newAngle + 360)%360
+    if(newAngle >= 180):
+        newAngle -= 360
+    return newAngle

Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py

@@ -58,7 +58,7 @@ def generateWaypoints():
         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)
+            waypointHeading = (45.0*i)%360.0
 
             # waypoints
             waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseWaypointAltitude + d*waypointAltitudeIncrement, waypointHeading]})

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

@@ -25,6 +25,7 @@
 * SOFTWARE.
 *
 */
+#include <math.h>       /* atan2 */
 #include <dji_osdk_ros/bus_driver.h>
 #include <dji_osdk_ros/FlightTaskControl.h>
 #include <dji_osdk_ros/ObtainControlAuthority.h>
@@ -53,10 +54,14 @@ ros::Publisher         syncPub; // thing to publish to sync topic "positionSync"
 
 sensor_msgs::NavSatFix gps_pos; // gps position from drone
 geometry_msgs::Transform ex_pos; // position from external source (qualisys or some such)
+float externalYaw; // yaw as measured by pos ref
+float externalPitch; // pitch as measured by pos ref
+float externalRoll; // roll as measured by pos ref
 bool                   gps_home_flag = false;
 bool                   pos_home_flag = false;
 bool                   positionMissionFlag = false;
 bool                   waypointMissionFlag = false;
+bool                   positionPlaybackParam = false;
 ros::Subscriber        gps_pos_subscriber;
 ros::Subscriber        pos_feedback_subscriber;
 int                    droneId; // stupid but will work for now
@@ -74,6 +79,8 @@ long double            initialPositionDistance; // distance to first position
 double                 baseSpeedParam;
 double                 minSpeedParam;
 double                 speedScalarParam;
+double                 minYawSpeedParam;
+double                 maxYawSpeedParam;
 
 std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
 float poseMagnitudeScaler; // need to run calibration to get
@@ -92,6 +99,17 @@ long double toRadians(const long double degree)
   return (one_deg * degree);
 }
 
+float normalizeAngle(float angle)
+{
+  float newAngle = std::fmod(angle, (2.0*M_PI));
+  newAngle = std::fmod((newAngle + (2.0*M_PI)), (2.0*M_PI));
+  if(newAngle >= M_PI)
+  {
+    newAngle -= (2.0*M_PI);
+  }
+  return newAngle;
+}
+
 float eucDistance(std::vector<float> a, std::vector<float> b)
 {
   // do the math bb
@@ -303,6 +321,12 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
 {
   ex_pos = *msg; // alllllways load the message
 
+  // FINE ill DO it MYself all ALONE
+  externalYaw = atan2(2.0*(ex_pos.rotation.x*ex_pos.rotation.y + ex_pos.rotation.z*ex_pos.rotation.w), 1 - 2.0*(ex_pos.rotation.y*ex_pos.rotation.y + ex_pos.rotation.z*ex_pos.rotation.z));
+  externalPitch = asin(2.0*(ex_pos.rotation.x*ex_pos.rotation.z - ex_pos.rotation.w*ex_pos.rotation.y));
+  externalRoll = atan2(2.0*(ex_pos.rotation.x*ex_pos.rotation.w + ex_pos.rotation.y*ex_pos.rotation.z), 1 - 2.0*(ex_pos.rotation.z*ex_pos.rotation.z + ex_pos.rotation.w*ex_pos.rotation.w));
+
+  // ROS_INFO("current (roll pitch yaw): (%f, %f, %f)", externalRoll, externalPitch, externalYaw);
   if (!positionMissionFlag)
   {
     return; // peace out if not running mission
@@ -375,84 +399,168 @@ bool runWaypointMission()
 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())
+  if (positionPlaybackParam)
   {
-    // 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]
-    };
-
-    // 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?
+    // replaying a capture from reference WIP
+          // ros::Rate updateRate(20); // update period for topics/commands
+          // 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]
+          //   };
+          //
+          //   if (!rotationMatrixCalibratedFlag) // could also do this before loop?
+          //   {
+          //     // run rotation matrix calc
+          //     calibratePositionReference();
+          //     rotationMatrixCalibratedFlag = true;
+          //   }
+          //
+          //   // may need to rotate this yaw to match coordinate system
+          //   // Eigen::Quaterniond 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);
+          //   float targetYaw = applyRotationAngle(normalizeAngle((M_PI/180.0)*tempPos[4]));
+          //   float currentYaw = externalYaw;
+          //   ROS_INFO("target yaw: %f", targetYaw);
+          //   ROS_INFO("current yaw: %f", currentYaw);
+          //
+          //   float yawCommand = normalizeAngle(currentYaw - targetYaw);
+          //   // check dir we should go
+          //   ROS_INFO("yaw command: %f", yawCommand);
+          //
+          //   // convert both target and current position, and move between
+          //   std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
+          //   std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
+          //   // map yaw command (in deg) to rotation offset (in rads)
+          //   // float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
+          //
+          //   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], yawCommand);
+          //
+          //   ros::spinOnce();
+          //
+          //   // check if we're close to position
+          //   float checkDist = eucDistance(currentPos, targetPos);
+          //   if (checkDist <= 0.2 && abs(currentYaw - targetYaw) <= 0.1) // close ish
+          //   {
+          //     currentPositionIndex += 1; // go to next position
+          //   }
+          //
+          //   if (currentPositionIndex >= numPositions)
+          //   {
+          //     // leave
+          //     positionMissionFlag = false;
+          //   }
+          //   // ROS_INFO("distance to first position: %f", firstPosDist);
+          //
+          //
+          //   updateRate.sleep();
+          // }
+  }
+  else
+  {
+    // executing a set of positions in reference
+    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())
     {
-      // run rotation matrix calc
-      calibratePositionReference();
-      rotationMatrixCalibratedFlag = true;
-    }
+      // 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]
+      };
 
-    // convert both target and current position, and move between
-    std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
-    std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
-    // map yaw command (in deg) to rotation offset (in rads)
-    // float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
+      if (!rotationMatrixCalibratedFlag) // could also do this before loop?
+      {
+        // run rotation matrix calc
+        calibratePositionReference();
+        rotationMatrixCalibratedFlag = true;
+      }
 
-    std::vector<float> moveCommand
-    {
-      targetPosMapped[0] - currentPosMapped[0],
-      targetPosMapped[1] - currentPosMapped[1],
-      targetPosMapped[2] - currentPosMapped[2]
-    };
+      // may need to rotate this yaw to match coordinate system
+      // Eigen::Quaterniond 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);
+      float targetYaw = applyRotationAngle(normalizeAngle((M_PI/180.0)*tempPos[4]));
+      float currentYaw = externalYaw;
+      ROS_INFO("target yaw: %f", targetYaw);
+      ROS_INFO("current yaw: %f", currentYaw);
+
+      float yawCommand = normalizeAngle(currentYaw - targetYaw);
+      // check dir we should go
+      ROS_INFO("yaw command: %f", yawCommand);
+
+      // convert both target and current position, and move between
+      std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
+      std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
+      // map yaw command (in deg) to rotation offset (in rads)
+      // float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
+
+      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], yawCommand);
+      // go to position
+      relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
 
-    ros::spinOnce();
+      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
-    }
+      // check if we're close to position
+      float checkDist = eucDistance(currentPos, targetPos);
+      if (checkDist <= 0.2 && abs(currentYaw - targetYaw) <= 0.1) // close ish
+      {
+        currentPositionIndex += 1; // go to next position
+      }
 
-    if (currentPositionIndex >= numPositions)
-    {
-      // leave
-      positionMissionFlag = false;
-    }
-    // ROS_INFO("distance to first position: %f", firstPosDist);
+      if (currentPositionIndex >= numPositions)
+      {
+        // leave
+        positionMissionFlag = false;
+      }
+      // ROS_INFO("distance to first position: %f", firstPosDist);
 
 
-    updateRate.sleep();
+      updateRate.sleep();
+    }
   }
+
   return true;
 }
 
@@ -593,17 +701,16 @@ bool land()
   return flightTaskControl.response.result;
 }
 
-float applyRotationAngle(float targetYaw)
+// converts local yaw in rads to mapped yaw in rads
+float applyRotationAngle(float localYaw)
 {
-  // make unit vector out of yaw, rotate, dot with pos x, get angle
-  const float sinA = sinf( (M_PI/180.0)*targetYaw );
-  const float cosA = cosf( (M_PI/180.0)*targetYaw );
-  std::vector<float> targetDir = {cosA, sinA, 0.0};
-  std::vector<float> rotatedDir = MatrixMultiply(poseRotationMatrix, targetDir);
-  // convert back into angle
-  std::vector<float> referenceAxis = {1.0, 0.0, 0.0};
-  float rotatedYaw = dotProduct(referenceAxis, rotatedDir);
-  return (180.0/M_PI)*acosf(rotatedYaw);
+  if (!rotationMatrixCalibratedFlag) // could also do this before loop?
+  {
+    // run rotation matrix calc
+    calibratePositionReference();
+    rotationMatrixCalibratedFlag = true;
+  }
+  return localYaw + poseRotationRadians;
 }
 
 bool relativePosition(float x, float y, float z, float yaw)
@@ -613,16 +720,40 @@ bool relativePosition(float x, float y, float z, float yaw)
   float totalDist = Magnitude3(posVec);
   float speed = min(speedScalarParam*pow(totalDist,2) + minSpeedParam, baseSpeedParam);
 
+  // float yawSpeed = max(min(abs(yaw), static_cast<float>(maxYawSpeedParam)), static_cast<float>(minYawSpeedParam));
+
   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};
 
   dji_osdk_ros::FlightTaskControl flightTaskControl;
   dji_osdk_ros::JoystickParams joystickParams;
+
+  // float sentYaw = ((abs(yaw)/yaw)*yawSpeed)/(totalTimeMs/1000.0);
+
   joystickParams.x = velVec[0];
   joystickParams.y = velVec[1];
   joystickParams.z = velVec[2];
-  joystickParams.yaw = yaw/(totalTimeMs/1000.0); // assuming this is in rads/sec??
+  if (abs(yaw) >= 0.01)
+  {
+    float tempYawRate = (180.0/M_PI)*yaw/(totalTimeMs/1000.0);
+    if (abs(tempYawRate) > maxYawSpeedParam)
+    {
+      tempYawRate = (tempYawRate/abs(tempYawRate))*maxYawSpeedParam;
+      // if distance is small, let yaw dictate duration
+      if (totalDist <= 0.1)
+      {
+        totalTimeMs = (abs(yaw)/tempYawRate)*1000.0;
+      }
+    }
+    ROS_INFO("yaw rate: %f over %f sec", tempYawRate, (totalTimeMs/1000.0));
+    ROS_INFO("---------------\n");
+    joystickParams.yaw = tempYawRate;
+  }
+  else
+  {
+    joystickParams.yaw = 0.0;
+  }
   flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL;
   flightTaskControl.request.joystickCommand = joystickParams;
   flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
@@ -689,14 +820,6 @@ void calibratePositionReference()
   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)
@@ -789,8 +912,11 @@ int main(int argc, char** argv)
 
   ros::param::get("base_speed", baseSpeedParam);
   ros::param::get("min_speed", minSpeedParam);
+  // ros::param::get("min_yaw_speed", minYawSpeedParam);
+  ros::param::get("max_yaw_speed", maxYawSpeedParam);
   ros::param::get("speed_scalar", speedScalarParam);
   ros::param::get("drone_id", droneId);
+  ros::param::get("position_playback", positionPlaybackParam);
   // ROS_INFO("base speed set to %f", baseSpeedParam);
 
   // ROS stuff