relative positioning tweaks

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

@@ -501,7 +501,7 @@ bool runPositionMission()
       ROS_INFO("Elapsed frame time: %f", static_cast<float>(dt));
 
       float yawCommand = normalizeAngle(currentYaw - targetYaw);
-      yawCommand = 0.0; // HACK
+      // yawCommand = 0.0; // HACK
       // check dir we should go
       ROS_INFO("yaw command: %f", yawCommand);
 
@@ -543,10 +543,16 @@ bool runPositionMission()
       //   std::vector<float> tempVelocity = normalizeVector(velocityCommand);
       //   velocityCommand = std::vector<float> {static_cast<float>(maxSpeedParam*tempVelocity[0]), static_cast<float>(maxSpeedParam*tempVelocity[1]), static_cast<float>(maxSpeedParam*tempVelocity[2])};
       // }
+      if (Magnitude3(velocityCommand) > maxSpeedParam)
+      {
+        velocityCommand = normalizeVector(velocityCommand);
+        velocityCommand = std::vector<float> {maxSpeedParam*velocityCommand[0], maxSpeedParam*velocityCommand[1], maxSpeedParam*velocityCommand[2]};
+        ROS_INFO("normalizing velocity command...");
+      }
       ROS_INFO("calling vel command %f, %f, %f", velocityCommand[0], velocityCommand[1], velocityCommand[2]);
       relativePosition(velocityCommand[0], velocityCommand[1], velocityCommand[2], yawCommand);
 
-      updateRate.sleep();
+      // updateRate.sleep();
 
       // check if position call was correct in terms of distance TODO also do this process for the rotation matrix
       // ---------------------------------
@@ -569,6 +575,7 @@ bool runPositionMission()
   }
   else
   {
+    std::vector<float> velocityCommand {0.0, 0.0, 0.0};
     // executing a set of positions in reference
     int numPositions;
     ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions);
@@ -587,34 +594,43 @@ bool runPositionMission()
       kdg = (kd + kp*dt)*g;
       previousWallTime = ros::WallTime::now();
 
-
       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> currentVel
+      {
+        static_cast<float>(ex_pos.translation.x - previous_ex_pos.translation.x),
+        static_cast<float>(ex_pos.translation.y - previous_ex_pos.translation.y),
+        static_cast<float>(ex_pos.translation.z - previous_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[1],
         tempPos[2],
         tempPos[3]
       };
 
-      if (!rotationMatrixCalibratedFlag) // could also do this before loop?
-      {
-        // run rotation matrix calc
-        std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
-        std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
-        calibratePositionReference(requestedPosition, actualPosition);
-        rotationMatrixCalibratedFlag = true;
-      }
+      std::vector<float> targetVel {0.0, 0.0, 0.0};
+
+      // if (!rotationMatrixCalibratedFlag) // could also do this before loop?
+      // {
+      //   // run rotation matrix calc
+      //   std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
+      //   std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
+      //   calibratePositionReference(requestedPosition, actualPosition);
+      //   rotationMatrixCalibratedFlag = true;
+      // }
 
       // may need to rotate this yaw to match coordinate system
-      float targetYaw = applyRotationAngle(normalizeAngle((M_PI/180.0)*tempPos[4]));
+      float targetYaw = normalizeAngle((M_PI/180.0)*tempPos[4]);
       float currentYaw = feedbackYaw;
       ROS_INFO("current postion %f, %f, %f", currentPos[0], currentPos[1], currentPos[2]);
       ROS_INFO("target position %f, %f, %f", targetPos[0], targetPos[1], targetPos[2]);
@@ -627,24 +643,54 @@ bool runPositionMission()
 
       // convert both target and current position, and move between
       // ROS_INFO("applying rotation matrix scaling...");
-      std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
-      std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
+      // std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
+      // std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
       // ROS_INFO("successfully applied rotation matrix");
       // 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
+      std::vector<float> force
       {
-        targetPosMapped[0] - currentPosMapped[0],
-        targetPosMapped[1] - currentPosMapped[1],
-        targetPosMapped[2] - currentPosMapped[2]
+        // using commanded velocity instead of actual
+        static_cast<float>(((targetPos[0] - currentPos[0])*ksg + (targetVel[0] - velocityCommand[0])*kdg)*speedScalarParam),
+        static_cast<float>(((targetPos[1] - currentPos[1])*ksg + (targetVel[1] - velocityCommand[1])*kdg)*speedScalarParam),
+        static_cast<float>(((targetPos[2] - currentPos[2])*ksg + (targetVel[2] - velocityCommand[2])*kdg)*speedScalarParam)
       };
 
+      ROS_INFO("target velocity: %f, %f, %f", targetVel[0], targetVel[1], targetVel[2]);
+      ROS_INFO("current velocity: %f, %f, %f", currentVel[0], currentVel[1], currentVel[2]);
+
       // go to position
-      ROS_INFO("calling move command w %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
-      relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
+      // ROS_INFO("calling move command %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
+
+      velocityCommand = std::vector<float> {velocityCommand[0] + dt*force[0], velocityCommand[1] + dt*force[1], velocityCommand[2] + dt*force[2]};
+      // if (Magnitude3(velocityCommand) > maxSpeedParam)
+      // {
+      //   ROS_INFO("bad force val %f, %f, %f", force[0], force[1], force[2]);
+      //   std::vector<float> tempVelocity = normalizeVector(velocityCommand);
+      //   velocityCommand = std::vector<float> {static_cast<float>(maxSpeedParam*tempVelocity[0]), static_cast<float>(maxSpeedParam*tempVelocity[1]), static_cast<float>(maxSpeedParam*tempVelocity[2])};
+      // }
+      if (Magnitude3(velocityCommand) > maxSpeedParam)
+      {
+        velocityCommand = normalizeVector(velocityCommand);
+        velocityCommand = std::vector<float> {maxSpeedParam*velocityCommand[0], maxSpeedParam*velocityCommand[1], maxSpeedParam*velocityCommand[2]};
+        ROS_INFO("normalizing velocity command...");
+      }
+      ROS_INFO("calling vel command %f, %f, %f", velocityCommand[0], velocityCommand[1], velocityCommand[2]);
+      relativePosition(velocityCommand[0], velocityCommand[1], velocityCommand[2], yawCommand);
+
+      // 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)
+      {
+        positionMissionFlag = false;
+      }
 
-      updateRate.sleep();
+      // updateRate.sleep();
       // check if position call was correct in terms of distance TODO also do this process for the rotation matrix
       // ---------------------------------
       // std::vector<float> calCheckPosition
@@ -665,16 +711,6 @@ bool runPositionMission()
       //
       // updateMagnitudeScaler(refDist/cmdDist);
       // // ---------------------------------
-      // // check if we're close to position
-      // float checkDist = eucDistance(currentPos, targetPos);
-      // if (checkDist <= 0.4) //&& abs(currentYaw - targetYaw) <= 0.1) // close ish
-      // {
-      //   currentPositionIndex += 1; // go to next position
-      // }
-      // if (currentPositionIndex >= numPositions)
-      // {
-      //   positionMissionFlag = false;
-      // }
     }
   }
 
@@ -818,6 +854,7 @@ bool land()
   return flightTaskControl.response.result;
 }
 
+
 // converts local yaw in rads to mapped yaw in rads
 float applyRotationAngle(float localYaw)
 {
@@ -872,17 +909,17 @@ bool relativePosition(float x, float y, float z, float yaw)
   joystickParams.x = velVec[0];
   joystickParams.y = velVec[1];
   joystickParams.z = velVec[2];
-  if (abs(yaw) >= 0.01)
+  if (abs(yaw) >= 0.05)
   {
     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;
-      }
+      // if (totalDist <= 0.1)
+      // {
+      //   totalTimeMs = (abs(yaw)/tempYawRate)*1000.0;
+      // }
     }
     joystickParams.yaw = tempYawRate;
   }
@@ -923,8 +960,11 @@ bool stopMotion()
 
 bool goHome()
 {
-  // TODO fix this pls
-  return true;
+  dji_osdk_ros::FlightTaskControl flightTaskControl;
+  flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_GOHOME_AND_CONFIRM_LANDING;
+  flight_control_client.call(flightTaskControl);
+
+  return flightTaskControl.response.result;
 }