Skip to content

D
Drone ROS Packages
Commit 9325d37e authored by Larkin Heintzman's avatar Larkin Heintzman
Browse files
Options

new controller tweaks

parent 67aec53a
Hide whitespace changes
Inline Side-by-side
Showing with 150 additions and 317 deletions
Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h
View file @ 9325d37e
......@@ -111,7 +111,7 @@ ServiceAck obtainCtrlAuthority();
bool takeoff();
bool relativePosition(float x, float y, float z, float yaw);
std::vector<float> relativePosition(float x, float y, float z, float yaw);
float applyRotationAngle(float targetYaw);
......@@ -127,10 +127,12 @@ void calibratePositionReference(std::vector<float> requestedPosition, std::vecto
void updateMagnitudeScaler(float newValue);
std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec);
float flatAngleBetween(std::vector<float> a, std::vector<float> b);
void updateAngleScaler(float newValue);
bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res);
std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec);
// std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec);
void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg);
......
Onboard-SDK-ROS/scripts/qualisysFeedback.py
View file @ 9325d37e
......@@ -25,7 +25,7 @@ def main():
name = rospy.get_param("~trackName")
namespace = rospy.get_param("~qualisysName")
rospy.loginfo("looking for track of name " + str(name) + " ...")
pub = rospy.Publisher(namespace + "/pose_feedback", Transform, queue_size = 1)
pub = rospy.Publisher(namespace, Transform, queue_size = 1)
rospy.Subscriber("/" + namespace + "/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
rospy.spin()
......
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
View file @ 9325d37e
......@@ -58,14 +58,16 @@ geometry_msgs::Transform ex_pos; // position from external source (qualisys or s
geometry_msgs::Transform track_pos; // position from external source (qualisys or some such)
geometry_msgs::Transform previous_ex_pos; // position from external source (qualisys or some such)
geometry_msgs::Transform previous_track_pos; // position from external source (qualisys or some such)
std::vector<float> expectedVel; // expected position after applying relative position move
float feedbackYaw; // yaw as measured by pos ref
float feedbackPitch; // pitch as measured by pos ref
float feedbackRoll; // roll as measured by pos ref
float trackingYaw; // yaw to track
float trackingPitch; // pitch to track
float trackingRoll; // roll to track
int calibrationWindow = 5; // the last _ position calls that are included in coordinate system calibration
int calibrationWindow = 25; // the last _ position calls that are included in coordinate system calibration
std::vector<float> calibrationWindowValues;
float angleOffset;
float controlLoopTime;
float updateRateParam;
std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
......@@ -206,6 +208,15 @@ float dotProduct(std::vector<float> vect_A, std::vector<float> vect_B)
return product;
}
float flatAngleBetween(std::vector<float> a, std::vector<float> b)
{
// flatten each vector into the horizontal plane because im lazy
std::vector<float> aFlat = std::vector<float> {a[0], a[1], 0.0};
std::vector<float> bFlat = std::vector<float> {b[0], b[1], 0.0};
float dotProd = dotProduct(aFlat, bFlat);
return normalizeAngle(acosf(dotProd/(Magnitude3(a)*Magnitude3(b))));
}
// Function to find
// cross product of two vector array.
std::vector<float> crossProduct(std::vector<float> vect_A, std::vector<float> vect_B)
......@@ -365,11 +376,6 @@ void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg)
trackingPitch = asin(2.0*(track_pos.rotation.x*track_pos.rotation.z - track_pos.rotation.w*track_pos.rotation.y));
trackingRoll = atan2(2.0*(track_pos.rotation.x*track_pos.rotation.w + track_pos.rotation.y*track_pos.rotation.z), 1 - 2.0*(track_pos.rotation.z*track_pos.rotation.z + track_pos.rotation.w*track_pos.rotation.w));
// update wall time for tracking trajectory
// ros::param::get("controller_damping", damping);
// ros::param::get("controller_frequency", frequency);
// kp = (18.0*frequency*frequency)/2.0;
// kd = (9.0/2.0)*frequency*damping;
}
bool setupWaypointMission(int responseTimeout)
......@@ -435,249 +441,86 @@ bool runWaypointMission()
bool runPositionMission()
{
std::vector<float> velocityCommand {0.0, 0.0, 0.0};
if (positionPlaybackParam)
ros::Rate updateRate(updateRateParam); // update period for topics/commands
positionMissionFlag = true;
int currentPositionIndex = 0;
int numPositions;
ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions);
while (positionMissionFlag && ros::ok())
{
// replaying a capture from reference
// executing a set of positions in reference
ros::Rate updateRate(updateRateParam); // update period for topics/commands
positionMissionFlag = true;
// let topics update
ros::spinOnce();
dt = (ros::WallTime::now() - previousWallTime).toNSec()*1e-9;
g = 1/(1 + kd*dt + kp*dt*dt);
ksg = kp*g;
kdg = (kd + kp*dt)*g;
previousWallTime = ros::WallTime::now();
while (positionMissionFlag && ros::ok())
std::vector<float> currentPos
{
// let topics update
ros::spinOnce();
static_cast<float>(ex_pos.translation.x),
static_cast<float>(ex_pos.translation.y),
static_cast<float>(ex_pos.translation.z)
};
dt = (ros::WallTime::now() - previousWallTime).toNSec()*1e-9;
g = 1/(1 + kd*dt + kp*dt*dt);
ksg = kp*g;
kdg = (kd + kp*dt)*g;
previousWallTime = ros::WallTime::now();
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> 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> targetPos;
std::vector<float> targetVel;
float targetYaw;
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)
};
// check expected velocity verses actual velocity
if (Magnitude3(velocityCommand) != 0.0 && Magnitude3(currentVel) != 0.0)
{
float ang = flatAngleBetween(velocityCommand, currentVel);
updateAngleScaler(ang);
ROS_INFO("new angle discrepancy!: %f", angleOffset);
}
std::vector<float> targetPos
// next depends on what mode we're in ------------------------------------
if (positionPlaybackParam)
{
targetPos = std::vector<float>
{
static_cast<float>(track_pos.translation.x),
static_cast<float>(track_pos.translation.y),
static_cast<float>(track_pos.translation.z)
};
std::vector<float> targetVel
targetVel = std::vector<float>
{
static_cast<float>(track_pos.translation.x - previous_track_pos.translation.x),
static_cast<float>(track_pos.translation.y - previous_track_pos.translation.y),
static_cast<float>(track_pos.translation.z - previous_track_pos.translation.z)
};
// 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;
// }
// float targetYaw = applyRotationAngle(trackingYaw); // might need to use different axis here, bc rotations
float targetYaw = trackingYaw; // might need to use different axis here, bc rotations
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]);
ROS_INFO("Target yaw: %f", targetYaw);
ROS_INFO("Current yaw: %f", currentYaw);
ROS_INFO("Elapsed frame time: %f", static_cast<float>(dt));
float yawCommand = normalizeAngle(currentYaw - targetYaw);
// yawCommand = 0.0; // HACK
// 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> targetVelMapped = applyRotationMatrixScaling(targetVel);
// std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// std::vector<float> currentVelMapped = applyRotationMatrixScaling(currentVel);
// map yaw command (in deg) to rotation offset (in rads)
// std::vector<float> moveCommand
// {
// targetPosMapped[0] - currentPosMapped[0],
// targetPosMapped[1] - currentPosMapped[1],
// targetPosMapped[2] - currentPosMapped[2]
// };
// inverse 2nd order controller edits!
// need: ksg, kdg, kp, kd, g, dt, and finally F
std::vector<float> force
{
// 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 %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);
// 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
// {
// static_cast<float>(ex_pos.translation.x),
// static_cast<float>(ex_pos.translation.y),
// static_cast<float>(ex_pos.translation.z)
// };
// // what the distance actually was in ref coords
// float refDist = eucDistance(currentPos, calCheckPosition);
// // what the distance was in command coords
// float cmdDist = Magnitude3(force);
// if (cmdDist > stdMoveDist)
// {
// cmdDist = stdMoveDist;
// }
// updateMagnitudeScaler(refDist/cmdDist);
targetYaw = trackingYaw;
}
}
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);
int currentPositionIndex = 0; // will increment
ros::Rate updateRate(updateRateParam); // update period for topics/commands
positionMissionFlag = true;
while (positionMissionFlag && ros::ok())
else
{
// let topics update
ros::spinOnce();
dt = (ros::WallTime::now() - previousWallTime).toNSec()*1e-9;
g = 1/(1 + kd*dt + kp*dt*dt);
ksg = kp*g;
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
{
targetPos = std::vector<float> {
tempPos[1],
tempPos[2],
tempPos[3]
};
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 = 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]);
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
// ROS_INFO("applying rotation matrix scaling...");
// 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> force
{
// 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]);
targetVel = std::vector<float> {0.0, 0.0, 0.0};
// go to position
// 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);
targetYaw = normalizeAngle((M_PI/180.0)*tempPos[4]);
// check if we're close to position
float checkDist = eucDistance(currentPos, targetPos);
......@@ -690,30 +533,41 @@ bool runPositionMission()
positionMissionFlag = false;
}
// 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
// {
// static_cast<float>(ex_pos.translation.x),
// static_cast<float>(ex_pos.translation.y),
// static_cast<float>(ex_pos.translation.z)
// };
// // what the distance actually was in ref coords
// float refDist = eucDistance(currentPos, calCheckPosition);
// // what the distance was in command coords
// float cmdDist = Magnitude3(moveCommand);
// if (cmdDist > stdMoveDist)
// {
// cmdDist = stdMoveDist;
// }
// // if movecommand, mapped to coords, does not match up with difference in position, scream
//
// updateMagnitudeScaler(refDist/cmdDist);
// // ---------------------------------
}
}
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]);
ROS_INFO("Target yaw: %f", targetYaw);
ROS_INFO("Current yaw: %f", currentYaw);
ROS_INFO("Elapsed frame time: %f", static_cast<float>(dt));
float yawCommand = normalizeAngle(currentYaw - targetYaw);
ROS_INFO("yaw command: %f", yawCommand);
std::vector<float> force
{
// 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]);
velocityCommand = std::vector<float> {velocityCommand[0] + dt*force[0], velocityCommand[1] + dt*force[1], velocityCommand[2] + dt*force[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...");
}
velocityCommand = MatrixMultiply(rotateAxisAngle(std::vector<float> {0.0, 0.0, 1.0}, angleOffset), velocityCommand);
ROS_INFO("calling vel command %f, %f, %f", velocityCommand[0], velocityCommand[1], velocityCommand[2]);
expectedVel = relativePosition(velocityCommand[0], velocityCommand[1], velocityCommand[2], yawCommand);
}
return true;
}
......@@ -854,37 +708,15 @@ bool land()
return flightTaskControl.response.result;
}
// converts local yaw in rads to mapped yaw in rads
float applyRotationAngle(float localYaw)
{
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;
}
return localYaw + poseRotationRadians;
}
bool relativePosition(float x, float y, float z, float yaw)
std::vector<float> 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);
// TESTING SMALL ANGLE ERROR IN PLACEMENT OF DRONE
// ------------------------------
// posVec = MatrixMultiply(rotateAxisAngle(std::vector<float> {0.0, 0.0, 1.0}, (M_PI/180.0)*180.0), posVec);
// normalize maximum distance to a parameter, say 1m steps so we can do position updates as we go
// if (totalDist > stdMoveDist)
// {
// // normalize the move to be of std length
// std::vector<float> normPos = normalizeVector(posVec);
// posVec[0] = normPos[0]*stdMoveDist;
// posVec[1] = normPos[1]*stdMoveDist;
// posVec[2] = normPos[2]*stdMoveDist;
// totalDist = stdMoveDist;
// }
float totalDist = Magnitude3(posVec);
float speed;
float totalTimeMs;
......@@ -938,7 +770,8 @@ bool relativePosition(float x, float y, float z, float yaw)
flightTaskControl.request.yawThresholdInDeg = 1;
flight_control_client.call(flightTaskControl); // actually waits for drone to "reach" position
return flightTaskControl.response.result;
// return flightTaskControl.response.result;
return std::vector<float> {(totalTimeMs/1000.0)*velVec[0], (totalTimeMs/1000.0)*velVec[1], (totalTimeMs/1000.0)*velVec[2]};
}
bool stopMotion()
......@@ -968,7 +801,21 @@ bool goHome()
}
void updateMagnitudeScaler(float newValue)
// void updateMagnitudeScaler(float newValue)
// {
// // remove the last value and insert new one
// calibrationWindowValues.pop_back();
// auto firstIndex = calibrationWindowValues.begin();
// calibrationWindowValues.insert(firstIndex, newValue);
// float sumer = 0.0;
// for (int i = 0; i < calibrationWindowValues.size(); i++)
// {
// sumer += calibrationWindowValues[i];
// }
// poseMagnitudeScaler = sumer/calibrationWindowValues.size();
// }
void updateAngleScaler(float newValue)
{
// remove the last value and insert new one
calibrationWindowValues.pop_back();
......@@ -979,7 +826,16 @@ void updateMagnitudeScaler(float newValue)
{
sumer += calibrationWindowValues[i];
}
poseMagnitudeScaler = sumer/calibrationWindowValues.size();
// check for first iteration, everything else is zero
// if (sumer == newValue)
// {
// // fill up with first value??
// for (int i = 0; i < calibrationWindowValues.size(); i++)
// {
// calibrationWindowValues[i] = newValue;
// }
// }
angleOffset = sumer/calibrationWindowValues.size();
}
void calibratePositionReference(std::vector<float> requestedPosition, std::vector<float> actualPosition)
......@@ -1005,18 +861,6 @@ void calibratePositionReference(std::vector<float> requestedPosition, std::vecto
ROS_INFO("pose magnitude scalar %f", poseMagnitudeScaler);
}
std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec)
{
std::vector<float> rotatedVec = MatrixMultiply(poseRotationMatrix, inputVec);
std::vector<float> outputVec
{
rotatedVec[0]*poseMagnitudeScaler,
rotatedVec[1]*poseMagnitudeScaler,
rotatedVec[2]*poseMagnitudeScaler
};
return outputVec;
}
// bool add(beginner_tutorials::AddTwoInts::Request &req, beginner_tutorials::AddTwoInts::Response &res)
bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res)
{
......@@ -1072,15 +916,15 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
takeoff();
break;
}
case 7:
{
ROS_INFO("received overwatch: calibrate pos ref");
std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
calibratePositionReference(requestedPosition, actualPosition);
break;
}
// case 7:
// {
// ROS_INFO("received overwatch: calibrate pos ref");
//
// std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
// std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
// calibratePositionReference(requestedPosition, actualPosition);
// break;
// }
default:
break;
}
......@@ -1089,26 +933,6 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
return res.result;
}
std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec)
{
// move a bit and return diff in position
//ros::spinOnce(); // let callbacks update
geometry_msgs::Transform preT = ex_pos;
relativePosition(calibrationVec[0], calibrationVec[1], calibrationVec[2], 0.0);
ros::spinOnce(); // let callbacks update
geometry_msgs::Transform postT = ex_pos;
std::vector<float> deltaPosition // what we got
{
static_cast<float>(postT.translation.x) - static_cast<float>(preT.translation.x),
static_cast<float>(postT.translation.y) - static_cast<float>(preT.translation.y),
static_cast<float>(postT.translation.z) - static_cast<float>(preT.translation.z),
};
// go back
relativePosition(-calibrationVec[0], -calibrationVec[1], -calibrationVec[2], 0.0);
ROS_INFO("delta position from calibration: %f, %f, %f", deltaPosition[0], deltaPosition[1], deltaPosition[2]);
return deltaPosition;
}
int main(int argc, char** argv)
{
......@@ -1129,6 +953,13 @@ int main(int argc, char** argv)
ros::param::get("controller_frequency", frequency);
kp = (6.0*frequency)*(6.0*frequency)* 0.25;
kd = 4.5*frequency*damping;
expectedVel = std::vector<float> {0.0, 0.0, 0.0};
angleOffset = 0.0;
calibrationWindowValues = std::vector<float> {};
for (int i = 0; i < calibrationWindow; i++)
{
calibrationWindowValues.push_back(0.0);
}
// kp = (18.0*frequency*frequency)/2.0;
// kd = (9.0/2.0)*frequency*damping;
......@@ -1150,8 +981,8 @@ int main(int argc, char** argv)
syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1);
gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>("dji_osdk_ros/gps_position", 1, &gpsPosCallback);
pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("feedback/pose_feedback", 1, &posFeedbackCallback);
pos_tracking_subscriber = nh.subscribe<geometry_msgs::Transform>("tracking/pose_feedback", 1, &posTrackingCallback);
pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("feedback", 1, &posFeedbackCallback);
pos_tracking_subscriber = nh.subscribe<geometry_msgs::Transform>("tracking", 1, &posTrackingCallback);
ros::ServiceServer overwatchService = nh.advertiseService("overwatch", overwatchFunction);
......
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