Skip to content

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

qualisys tracking changes

parent 1f96d6f8
Hide whitespace changes
Inline Side-by-side
Showing with 298 additions and 444 deletions
Onboard-SDK-ROS/launch/local_test.launch
View file @ a9a42489
<launch>
<rosparam command="load" param="/use_sim_time"> true </rosparam>
<!-- <rosparam file="$(find dji_osdk_ros)/launch/waypoints_musical_drones.json"/> -->
<node pkg="dji_osdk_ros" type="dji_vehicle_node" name="vehicle_node" output="screen">
<!-- node parameters -->
......@@ -36,8 +37,16 @@
</node> -->
<node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros">
<rosparam command="load" param="anchorPoint"> [37.196986, -80.578257] </rosparam>
<rosparam command="load" param="altitude"> 2.0 </rosparam>
<rosparam command="load" param="anchorPoint"> [37.196941, -80.578335] </rosparam>
<rosparam command="load" param="pathPoints">
[
[37.197879, -80.578022],
[37.198037, -80.577689],
[37.198161, -80.577359],
[37.198161, -80.577072],
[37.197879, -80.576637]
]
<rosparam command="load" param="altitude"> 10.0 </rosparam>
<rosparam command="load" param="droneNum"> 3 </rosparam>
</node>
......
Onboard-SDK-ROS/scripts/qualisysFeedback.py
View file @ a9a42489
......@@ -6,7 +6,9 @@ from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Transform
transform = Transform()
def callback(data):
global pub
# save the pose date in transform
transform.rotation = data.pose.orientation
# we must remapp
......@@ -14,17 +16,18 @@ def callback(data):
transform.translation.x = data.pose.position.x
transform.translation.y = data.pose.position.z
transform.translation.z = data.pose.position.y
pub.publish(transform)
def main():
global transform
global pub
rospy.init_node("qualisysFeedback", anonymous = True)
name = rospy.get_param("~trackName")
namespace = rospy.get_param("~qualisysName")
rospy.loginfo("looking for track of name " + str(name) + " ...")
rospy.Subscriber("/" + namespace + "/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
pub = rospy.Publisher(namespace + "/pose_feedback", Transform, queue_size = 1)
while not rospy.is_shutdown():
pub.publish(transform)
rospy.Subscriber("/" + namespace + "/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
rospy.spin()
if __name__ == '__main__':
main()
Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py
View file @ a9a42489
#!/usr/bin/python3
from tools import meters2lat_lon, lat_lon2meters
from tools import meters2lat_lon, lat_lon2meters, normalizeAngle
import numpy as np
import rospy
def normalizeAngle(angle):
newAngle = angle%360
newAngle = (newAngle + 360)%360
if(newAngle >= 180):
newAngle -= 360
return newAngle
def generateWaypoints():
rospy.init_node("waypointGenerator")
anchorPoint = rospy.get_param("~anchorPoint")
pathPoints = rospy.get_param("~pathPoints")
baseWaypointAltitude = rospy.get_param("~altitude")
droneNum = rospy.get_param("~droneNum")
waypointAltitudeIncrement = baseWaypointAltitude # increase altitude for each drone in team
# musical drones trajectory is convient test case, but general path is possible
# ring sizes
big = 5
med = 3
sml = 1
# decide if path is "preplanned" or not
if (len(pathPoints) != 0):
apMeters = lat_lon2meters(anchorPoint)
tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0}
waypointDict = {}
# define dictionary
numWaypoints = len(pathPoints)
numPositions = len(pathPoints)
apMeters = lat_lon2meters(anchorPoint)
# print(waypointDict)
waypointOffsets = [
[-big, -big],
[big, -big],
[big, big],
[med, med],
[-med, med],
[-med, -med],
[-sml, -sml],
[sml, -sml],
[sml, sml],
[big, -big],
]
tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0}
waypointDict = {}
# define dictionary
# for temp in range(droneNum):
# waypointDict["robot_{}".format(temp)] = tempDict
numWaypoints = len(waypointOffsets)
numPositions = len(waypointOffsets) # TODO, add position list to parameters
for d in range(droneNum):
# rospy.loginfo("blah!")
# print(d)
waypointDict["robot_{}".format(d)] = {}
waypointDict["robot_{}".format(d)] = {}
waypointDict["robot_{}".format(d)]["waypoints"] = {}
waypointDict["robot_{}".format(d)]["positions"] = {}
for i, pt in enumerate(pathPoints):
# for i, offset in enumerate(waypointOffsets):
waypointLatLon = pt
waypointHeading = 0.0
# 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)
positionXY = lat_lon2meters(apMeters) - lat_lon2meters(pt)
# print(waypointDict)
# waypoints
waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseWaypointAltitude + d*5, waypointHeading]})
# positions
waypointDict["robot_{}".format(d)]["positions"].update({"position_{}".format(i) : [i, positionXY[0], positionXY[1], baseWaypointAltitude + d*5, waypointHeading]})
for d in range(droneNum):
# rospy.loginfo("blah!")
# print(d)
waypointDict["robot_{}".format(d)] = {}
waypointDict["robot_{}".format(d)] = {}
waypointDict["robot_{}".format(d)]["waypoints"] = {}
waypointDict["robot_{}".format(d)]["positions"] = {}
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 = (45.0*i)%360.0
# do this betta
waypointDict["robot_{}".format(d)]["numWaypoints"] = numWaypoints
waypointDict["robot_{}".format(d)]["numPositions"] = numPositions
# print(waypointDict)
rospy.set_param("/robot_list", waypointDict)
else:
# musical drones trajectory is convient test case, but general path is possible
# ring sizes
big = 2.0
med = 2.0
sml = 2.0
# waypoints
waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseWaypointAltitude + d*waypointAltitudeIncrement, waypointHeading]})
# positions
waypointDict["robot_{}".format(d)]["positions"].update({"position_{}".format(i) : [i, offset[0], offset[1], baseWaypointAltitude + d*waypointAltitudeIncrement, waypointHeading]})
apMeters = lat_lon2meters(anchorPoint)
# do this betta
waypointDict["robot_{}".format(d)]["numWaypoints"] = numWaypoints
waypointDict["robot_{}".format(d)]["numPositions"] = numPositions
# print(waypointDict)
rospy.set_param("/robot_list", waypointDict)
waypointOffsets = [
[-big, -big],
[big, -big],
[big, big],
[med, med],
[-med, med],
[-med, -med],
[-sml, -sml],
[sml, -sml],
[sml, sml],
[big, -big],
]
tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0}
waypointDict = {}
# define dictionary
# for temp in range(droneNum):
# waypointDict["robot_{}".format(temp)] = tempDict
numWaypoints = len(waypointOffsets)
numPositions = len(waypointOffsets) # TODO, add position list to parameters
# print(waypointDict)
for d in range(droneNum):
# rospy.loginfo("blah!")
# print(d)
waypointDict["robot_{}".format(d)] = {}
waypointDict["robot_{}".format(d)] = {}
waypointDict["robot_{}".format(d)]["waypoints"] = {}
waypointDict["robot_{}".format(d)]["positions"] = {}
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)
# waypoints
waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseWaypointAltitude + d*waypointAltitudeIncrement, waypointHeading]})
# positions
waypointDict["robot_{}".format(d)]["positions"].update({"position_{}".format(i) : [i, offset[0], offset[1], baseWaypointAltitude + d*waypointAltitudeIncrement, waypointHeading]})
# do this betta
waypointDict["robot_{}".format(d)]["numWaypoints"] = numWaypoints
waypointDict["robot_{}".format(d)]["numPositions"] = numPositions
# print(waypointDict)
rospy.set_param("/robot_list", waypointDict)
return
# print(waypointDict)
if __name__ == "__main__":
......
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
View file @ a9a42489
......@@ -56,6 +56,8 @@ 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)
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)
float feedbackYaw; // yaw as measured by pos ref
float feedbackPitch; // pitch as measured by pos ref
float feedbackRoll; // roll as measured by pos ref
......@@ -64,7 +66,8 @@ float trackingPitch; // pitch to track
float trackingRoll; // roll to track
int calibrationWindow = 5; // the last _ position calls that are included in coordinate system calibration
std::vector<float> calibrationWindowValues;
float stdMoveDist;
float controlLoopTime;
float updateRateParam;
std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
std::vector<std::vector<std::vector<float>>> poseRotationMatrixValues; // need to run calibration to get
float poseMagnitudeScaler; // need to run calibration to get
......@@ -93,10 +96,23 @@ long double initialWaypointDistance; // distance to first waypoint
long double initialPositionDistance; // distance to first position
double baseSpeedParam;
double minSpeedParam;
double maxSpeedParam;
double speedScalarParam;
double minYawSpeedParam;
double maxYawSpeedParam;
// inverse controller variables
ros::WallTime previousWallTime;
double dt;
double damping;
double frequency;
double kp;
double kd;
double ksg;
double kdg;
double g;
// Utility function for
// converting degrees to radians
long double toRadians(const long double degree)
......@@ -329,6 +345,7 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
{
previous_ex_pos = ex_pos;
ex_pos = *msg; // alllllways load the message
// FINE ill DO it MYself all ALONE
......@@ -339,12 +356,20 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg)
{
previous_track_pos = track_pos;
track_pos = *msg;
// FINE ill DO it MYself all ALONE
trackingYaw = atan2(2.0*(track_pos.rotation.x*track_pos.rotation.y + track_pos.rotation.z*track_pos.rotation.w), 1 - 2.0*(track_pos.rotation.y*track_pos.rotation.y + track_pos.rotation.z*track_pos.rotation.z));
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)
......@@ -410,16 +435,25 @@ bool runWaypointMission()
bool runPositionMission()
{
std::vector<float> velocityCommand {0.0, 0.0, 0.0};
if (positionPlaybackParam)
{
// replaying a capture from reference WIP
// replaying a capture from reference
// executing a set of positions in reference
ros::Rate updateRate(20); // update period for topics/commands
ros::Rate updateRate(updateRateParam); // update period for topics/commands
positionMissionFlag = true;
while (positionMissionFlag && ros::ok())
{
// 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),
......@@ -427,6 +461,13 @@ bool runPositionMission()
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> targetPos
{
static_cast<float>(track_pos.translation.x),
......@@ -434,66 +475,96 @@ bool runPositionMission()
static_cast<float>(track_pos.translation.z)
};
if (!rotationMatrixCalibratedFlag) // could also do this before loop?
std::vector<float> targetVel
{
// 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;
}
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)
};
float targetYaw = applyRotationAngle(trackingYaw); // might need to use different axis here, bc rotations
// 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("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> currentPosMapped = applyRotationMatrixScaling(currentPos);
// 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
// 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
{
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])};
// }
ROS_INFO("calling vel command %f, %f, %f", velocityCommand[0], velocityCommand[1], velocityCommand[2]);
relativePosition(velocityCommand[0], velocityCommand[1], velocityCommand[2], yawCommand);
// let topics update
ros::spinOnce();
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);
// ---------------------------------
updateRate.sleep();
// 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);
}
}
else
......@@ -502,11 +573,21 @@ 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
ros::Rate updateRate(updateRateParam); // update period for topics/commands
positionMissionFlag = true;
while (positionMissionFlag && ros::ok())
{
// 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),
......@@ -563,46 +644,37 @@ bool runPositionMission()
ROS_INFO("calling move command w %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
// let topics update
ros::spinOnce();
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);
// ---------------------------------
// 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)
{
// leave
positionMissionFlag = false;
}
// ROS_INFO("distance to first position: %f", firstPosDist);
updateRate.sleep();
// 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);
// // ---------------------------------
// // 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;
// }
}
}
......@@ -628,7 +700,7 @@ void setWaypointDefaults(WayPointSettings* wp)
void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask)
{
waypointTask.velocity_range = 2;
waypointTask.velocity_range = baseSpeedParam;
waypointTask.idle_velocity = baseSpeedParam;
waypointTask.action_on_finish = dji_osdk_ros::MissionWaypointTask::FINISH_NO_ACTION;
waypointTask.mission_exec_times = 1;
......@@ -767,19 +839,30 @@ bool relativePosition(float x, float y, float z, float yaw)
float totalDist = Magnitude3(posVec);
// normalize maximum distance to a parameter, say 1m steps so we can do position updates as we go
if (totalDist > stdMoveDist)
// 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 speed;
float totalTimeMs;
// first check if we can make it in the allotted time
if ((totalDist/controlLoopTime) > maxSpeedParam)
{
// 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;
speed = maxSpeedParam;
totalTimeMs = controlLoopTime*1000.0;
}
else
{
speed = max(minSpeedParam, static_cast<double>(totalDist/controlLoopTime));
totalTimeMs = (totalDist/speed)*1000.0; // convert from s to ms, aim just short of position
}
float speed = min(speedScalarParam*pow(totalDist,2) + minSpeedParam, baseSpeedParam);
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};
......@@ -808,7 +891,7 @@ bool relativePosition(float x, float y, float z, float yaw)
joystickParams.yaw = 0.0;
}
ROS_INFO("yaw rate: %f | speed: %f, over %f sec", joystickParams.yaw, speed, (totalTimeMs/1000.0));
ROS_INFO("yaw rate: %f, speed: %f, over %f sec, going %fm", joystickParams.yaw, speed, (totalTimeMs/1000.0), totalDist);
ROS_INFO("---------------\n");
flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL;
......@@ -816,7 +899,6 @@ bool relativePosition(float x, float y, float z, float yaw)
flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
flightTaskControl.request.posThresholdInM = 0.1;
flightTaskControl.request.yawThresholdInDeg = 1;
flight_control_client.call(flightTaskControl); // actually waits for drone to "reach" position
return flightTaskControl.response.result;
......@@ -858,11 +940,6 @@ void updateMagnitudeScaler(float newValue)
sumer += calibrationWindowValues[i];
}
poseMagnitudeScaler = sumer/calibrationWindowValues.size();
ROS_INFO("new magnitude scaler set: ");
for (int i = 0; i < calibrationWindowValues.size(); i++)
{
ROS_INFO("%d : %f", i, calibrationWindowValues[i]);
}
}
void calibratePositionReference(std::vector<float> requestedPosition, std::vector<float> actualPosition)
......@@ -975,7 +1052,7 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec)
{
// move a bit and return diff in position
ros::spinOnce(); // let callbacks update
//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
......@@ -988,6 +1065,7 @@ std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec)
};
// 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;
}
......@@ -999,13 +1077,22 @@ 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_speed", maxSpeedParam);
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::param::get("std_move_dist", stdMoveDist);
// ROS_INFO("base speed set to %f", baseSpeedParam);
ros::param::get("loop_rate", updateRateParam);
controlLoopTime = 1.0/updateRateParam;
// additions
ros::param::get("controller_damping", damping);
ros::param::get("controller_frequency", frequency);
kp = (6.0*frequency)*(6.0*frequency)* 0.25;
kd = 4.5*frequency*damping;
// kp = (18.0*frequency*frequency)/2.0;
// kd = (9.0/2.0)*frequency*damping;
previousWallTime = ros::WallTime::now();
// ROS stuff
// obtain control authority?
......
database_tools/CMakeLists.txt deleted 100644 → 0
View file @ 1f96d6f8
cmake_minimum_required(VERSION 3.0.2)
project(database_tools)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES database_tools
# CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/database_tools.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/database_tools_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_database_tools.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
database_tools/package.xml deleted 100644 → 0
View file @ 1f96d6f8
<?xml version="1.0"?>
<package format="2">
<name>database_tools</name>
<version>0.0.0</version>
<description>The database_tools package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="larkin@todo.todo">larkin</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/database_tools</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
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