Skip to content

D
Drone ROS Packages
Commit 67aec53a authored by Larkin Heintzman's avatar Larkin Heintzman
Browse files
Options

relative positioning tweaks

parent a9a42489
Show whitespace changes
Inline Side-by-side
Showing with 79 additions and 39 deletions
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
View file @ 67aec53a
......@@ -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;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment