yaw hooks added and tweaked relative vel

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

@@ -112,6 +112,8 @@ bool takeoff();
 
 bool relativePosition(float x, float y, float z, float yaw);
 
+float applyRotationAngle(float targetYaw);
+
 bool land();
 
 bool setRates();

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

@@ -74,6 +74,8 @@ double                 speedScalarParam;
 
 std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
 float poseMagnitudeScaler; // need to run calibration to get
+float poseRotationRadians;
+bool rotationMatrixCalibratedFlag = false;
 
 // Utility function for
 // converting degrees to radians
@@ -133,7 +135,6 @@ std::vector<std::vector<float>> rotateAxisAngle(std::vector<float> u, float angl
   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
   {
@@ -220,14 +221,12 @@ std::vector<std::vector<float>> rotateAlign(std::vector<float> v1, std::vector<f
     {
       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
+    poseRotationRadians = acosf(dotProd);
+    std::vector<std::vector<float>> result = rotateAxisAngle(axis, poseRotationRadians); // 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);
+    ROS_INFO("angle rad: (%f)", poseRotationRadians);
     return result;
 }
 
@@ -398,18 +397,22 @@ bool runPositionMission()
       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]);
+    // float targetYaw = tempPos[4]; // TODO: need yaw measurement for this to work1!!!!1!
+    // float currentYaw = ex_pos.rotation.
     // map to our coordinate system
-    if (!ros::param::has("poseRotationMatrix/"))
+
+    if (!rotationMatrixCalibratedFlag) // could also do this before loop?
     {
       // run rotation matrix calc
       calibratePositionReference();
+      rotationMatrixCalibratedFlag = true;
     }
 
     // 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
     {
@@ -482,7 +485,7 @@ std::vector<DJI::OSDK::WayPointSettings> loadWaypoints()
   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]);
+    // 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;
@@ -544,22 +547,22 @@ ServiceAck missionAction(DJI::OSDK::DJI_MISSION_TYPE type,
 {
   dji_osdk_ros::MissionWpAction missionWpAction;
   dji_osdk_ros::MissionHpAction missionHpAction;
-  switch (type)
+  missionWpAction.request.action = action;
+  waypoint_action_client.call(missionWpAction);
+  if (!missionWpAction.response.result)
   {
-    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 };
+    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 { static_cast<bool>(missionWpAction.response.result),
+    missionWpAction.response.cmd_set,
+    missionWpAction.response.cmd_id,
+    missionWpAction.response.ack_data };
+  // switch (type)
+  // {
+  //   case DJI::OSDK::WAYPOINT:
+  // }
 }
 
 bool takeoff()
@@ -580,34 +583,47 @@ bool land()
   return flightTaskControl.response.result;
 }
 
+float applyRotationAngle(float targetYaw)
+{
+  // 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);
+}
+
 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);
+  // 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
+  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);
+  // 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;
+  joystickParams.yaw = yaw; // for now until i have yaw measurement up
   flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL;
   flightTaskControl.request.joystickCommand = joystickParams;
   flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
@@ -667,7 +683,7 @@ void calibratePositionReference()
   float requestedPositionMag = Magnitude3(requestedPosition);
   float actualPositionMag = Magnitude3(actualPosition);
 
-  poseMagnitudeScaler = requestedPositionMag/actualPositionMag;
+  poseMagnitudeScaler = (requestedPositionMag/actualPositionMag)*0.8; // aim just short of actual scalar, better to under than over shoot
 
   ROS_INFO("pose magnitude scalar %f", poseMagnitudeScaler);
 
@@ -676,12 +692,12 @@ void calibratePositionReference()
   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);
+  // 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)
@@ -776,7 +792,7 @@ int main(int argc, char** argv)
   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);
+  // ROS_INFO("base speed set to %f", baseSpeedParam);
 
   // ROS stuff
   // obtain control authority?