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> <launch>
<rosparam command="load" param="/use_sim_time"> true </rosparam>
<!-- <rosparam file="$(find dji_osdk_ros)/launch/waypoints_musical_drones.json"/> --> <!-- <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 pkg="dji_osdk_ros" type="dji_vehicle_node" name="vehicle_node" output="screen">
<!-- node parameters --> <!-- node parameters -->
...@@ -36,8 +37,16 @@ ...@@ -36,8 +37,16 @@
</node> --> </node> -->
<node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros"> <node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros">
<rosparam command="load" param="anchorPoint"> [37.196986, -80.578257] </rosparam> <rosparam command="load" param="anchorPoint"> [37.196941, -80.578335] </rosparam>
<rosparam command="load" param="altitude"> 2.0 </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> <rosparam command="load" param="droneNum"> 3 </rosparam>
</node> </node>
......
Onboard-SDK-ROS/scripts/qualisysFeedback.py
View file @ a9a42489
...@@ -6,7 +6,9 @@ from geometry_msgs.msg import PoseStamped ...@@ -6,7 +6,9 @@ from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Transform from geometry_msgs.msg import Transform
transform = Transform() transform = Transform()
def callback(data): def callback(data):
global pub
# save the pose date in transform # save the pose date in transform
transform.rotation = data.pose.orientation transform.rotation = data.pose.orientation
# we must remapp # we must remapp
...@@ -14,17 +16,18 @@ def callback(data): ...@@ -14,17 +16,18 @@ def callback(data):
transform.translation.x = data.pose.position.x transform.translation.x = data.pose.position.x
transform.translation.y = data.pose.position.z transform.translation.y = data.pose.position.z
transform.translation.z = data.pose.position.y transform.translation.z = data.pose.position.y
pub.publish(transform)
def main(): def main():
global transform global pub
rospy.init_node("qualisysFeedback", anonymous = True) rospy.init_node("qualisysFeedback", anonymous = True)
name = rospy.get_param("~trackName") name = rospy.get_param("~trackName")
namespace = rospy.get_param("~qualisysName") namespace = rospy.get_param("~qualisysName")
rospy.loginfo("looking for track of name " + str(name) + " ...") 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) pub = rospy.Publisher(namespace + "/pose_feedback", Transform, queue_size = 1)
while not rospy.is_shutdown(): rospy.Subscriber("/" + namespace + "/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
pub.publish(transform) rospy.spin()
if __name__ == '__main__': if __name__ == '__main__':
main() main()
Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py
View file @ a9a42489
#!/usr/bin/python3 #!/usr/bin/python3
from tools import meters2lat_lon, lat_lon2meters from tools import meters2lat_lon, lat_lon2meters, normalizeAngle
import numpy as np import numpy as np
import rospy import rospy
def normalizeAngle(angle):
newAngle = angle%360
newAngle = (newAngle + 360)%360
if(newAngle >= 180):
newAngle -= 360
return newAngle
def generateWaypoints(): def generateWaypoints():
rospy.init_node("waypointGenerator") rospy.init_node("waypointGenerator")
anchorPoint = rospy.get_param("~anchorPoint") anchorPoint = rospy.get_param("~anchorPoint")
pathPoints = rospy.get_param("~pathPoints")
baseWaypointAltitude = rospy.get_param("~altitude") baseWaypointAltitude = rospy.get_param("~altitude")
droneNum = rospy.get_param("~droneNum") droneNum = rospy.get_param("~droneNum")
waypointAltitudeIncrement = baseWaypointAltitude # increase altitude for each drone in team waypointAltitudeIncrement = baseWaypointAltitude # increase altitude for each drone in team
# musical drones trajectory is convient test case, but general path is possible # decide if path is "preplanned" or not
# ring sizes if (len(pathPoints) != 0):
big = 5 apMeters = lat_lon2meters(anchorPoint)
med = 3 tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0}
sml = 1 waypointDict = {}
# define dictionary
numWaypoints = len(pathPoints)
numPositions = len(pathPoints)
apMeters = lat_lon2meters(anchorPoint) # print(waypointDict)
waypointOffsets = [ for d in range(droneNum):
[-big, -big], # rospy.loginfo("blah!")
[big, -big], # print(d)
[big, big], waypointDict["robot_{}".format(d)] = {}
[med, med], waypointDict["robot_{}".format(d)] = {}
[-med, med], waypointDict["robot_{}".format(d)]["waypoints"] = {}
[-med, -med], waypointDict["robot_{}".format(d)]["positions"] = {}
[-sml, -sml], for i, pt in enumerate(pathPoints):
[sml, -sml], # for i, offset in enumerate(waypointOffsets):
[sml, sml], waypointLatLon = pt
[big, -big], waypointHeading = 0.0
] # waypointLatLon = meters2lat_lon([apMeters[0] + offset[0], apMeters[1] + offset[1]])
tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0} # waypointLatLonRev = meters2lat_lon([apMeters[0] - offset[0], apMeters[1] - offset[1]])
waypointDict = {} # waypointHeading = normalizeAngle(45*i)
# define dictionary positionXY = lat_lon2meters(apMeters) - lat_lon2meters(pt)
# for temp in range(droneNum):
# waypointDict["robot_{}".format(temp)] = tempDict
numWaypoints = len(waypointOffsets)
numPositions = len(waypointOffsets) # TODO, add position list to parameters
# 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): # do this betta
# rospy.loginfo("blah!") waypointDict["robot_{}".format(d)]["numWaypoints"] = numWaypoints
# print(d) waypointDict["robot_{}".format(d)]["numPositions"] = numPositions
waypointDict["robot_{}".format(d)] = {} # print(waypointDict)
waypointDict["robot_{}".format(d)] = {} rospy.set_param("/robot_list", waypointDict)
waypointDict["robot_{}".format(d)]["waypoints"] = {} else:
waypointDict["robot_{}".format(d)]["positions"] = {} # musical drones trajectory is convient test case, but general path is possible
for i, offset in enumerate(waypointOffsets): # ring sizes
waypointLatLon = meters2lat_lon([apMeters[0] + offset[0], apMeters[1] + offset[1]]) big = 2.0
waypointLatLonRev = meters2lat_lon([apMeters[0] - offset[0], apMeters[1] - offset[1]]) med = 2.0
waypointHeading = (45.0*i)%360.0 sml = 2.0
# waypoints apMeters = lat_lon2meters(anchorPoint)
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 waypointOffsets = [
waypointDict["robot_{}".format(d)]["numWaypoints"] = numWaypoints [-big, -big],
waypointDict["robot_{}".format(d)]["numPositions"] = numPositions [big, -big],
# print(waypointDict) [big, big],
rospy.set_param("/robot_list", waypointDict) [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) # print(waypointDict)
if __name__ == "__main__": 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" ...@@ -56,6 +56,8 @@ ros::Publisher syncPub; // thing to publish to sync topic "positionSync"
sensor_msgs::NavSatFix gps_pos; // gps position from drone 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 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 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 feedbackYaw; // yaw as measured by pos ref
float feedbackPitch; // pitch as measured by pos ref float feedbackPitch; // pitch as measured by pos ref
float feedbackRoll; // roll as measured by pos ref float feedbackRoll; // roll as measured by pos ref
...@@ -64,7 +66,8 @@ float trackingPitch; // pitch to track ...@@ -64,7 +66,8 @@ float trackingPitch; // pitch to track
float trackingRoll; // roll to track float trackingRoll; // roll to track
int calibrationWindow = 5; // the last _ position calls that are included in coordinate system calibration int calibrationWindow = 5; // the last _ position calls that are included in coordinate system calibration
std::vector<float> calibrationWindowValues; 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<float>> poseRotationMatrix; // need to run calibration to get
std::vector<std::vector<std::vector<float>>> poseRotationMatrixValues; // 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 float poseMagnitudeScaler; // need to run calibration to get
...@@ -93,10 +96,23 @@ long double initialWaypointDistance; // distance to first waypoint ...@@ -93,10 +96,23 @@ long double initialWaypointDistance; // distance to first waypoint
long double initialPositionDistance; // distance to first position long double initialPositionDistance; // distance to first position
double baseSpeedParam; double baseSpeedParam;
double minSpeedParam; double minSpeedParam;
double maxSpeedParam;
double speedScalarParam; double speedScalarParam;
double minYawSpeedParam; double minYawSpeedParam;
double maxYawSpeedParam; 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 // Utility function for
// converting degrees to radians // converting degrees to radians
long double toRadians(const long double degree) long double toRadians(const long double degree)
...@@ -329,6 +345,7 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg) ...@@ -329,6 +345,7 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg) void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
{ {
previous_ex_pos = ex_pos;
ex_pos = *msg; // alllllways load the message ex_pos = *msg; // alllllways load the message
// FINE ill DO it MYself all ALONE // FINE ill DO it MYself all ALONE
...@@ -339,12 +356,20 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg) ...@@ -339,12 +356,20 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg) void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg)
{ {
previous_track_pos = track_pos;
track_pos = *msg; track_pos = *msg;
// FINE ill DO it MYself all ALONE // 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)); 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)); 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)); 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) bool setupWaypointMission(int responseTimeout)
...@@ -410,16 +435,25 @@ bool runWaypointMission() ...@@ -410,16 +435,25 @@ bool runWaypointMission()
bool runPositionMission() bool runPositionMission()
{ {
std::vector<float> velocityCommand {0.0, 0.0, 0.0};
if (positionPlaybackParam) if (positionPlaybackParam)
{ {
// replaying a capture from reference WIP // replaying a capture from reference
// executing a set of positions in 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; positionMissionFlag = true;
while (positionMissionFlag && ros::ok()) 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 std::vector<float> currentPos
{ {
static_cast<float>(ex_pos.translation.x), static_cast<float>(ex_pos.translation.x),
...@@ -427,6 +461,13 @@ bool runPositionMission() ...@@ -427,6 +461,13 @@ bool runPositionMission()
static_cast<float>(ex_pos.translation.z) 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 std::vector<float> targetPos
{ {
static_cast<float>(track_pos.translation.x), static_cast<float>(track_pos.translation.x),
...@@ -434,66 +475,96 @@ bool runPositionMission() ...@@ -434,66 +475,96 @@ bool runPositionMission()
static_cast<float>(track_pos.translation.z) static_cast<float>(track_pos.translation.z)
}; };
if (!rotationMatrixCalibratedFlag) // could also do this before loop? std::vector<float> targetVel
{ {
// run rotation matrix calc static_cast<float>(track_pos.translation.x - previous_track_pos.translation.x),
std::vector<float> requestedPosition = {0.5, 0.0, 0.0}; static_cast<float>(track_pos.translation.y - previous_track_pos.translation.y),
std::vector<float> actualPosition = DoCalibrationMove(requestedPosition); static_cast<float>(track_pos.translation.z - previous_track_pos.translation.z)
calibratePositionReference(requestedPosition, actualPosition); };
rotationMatrixCalibratedFlag = true;
}
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; float currentYaw = feedbackYaw;
ROS_INFO("current postion %f, %f, %f", currentPos[0], currentPos[1], currentPos[2]); 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 position %f, %f, %f", targetPos[0], targetPos[1], targetPos[2]);
ROS_INFO("target yaw: %f", targetYaw); ROS_INFO("Target yaw: %f", targetYaw);
ROS_INFO("current yaw: %f", currentYaw); ROS_INFO("Current yaw: %f", currentYaw);
ROS_INFO("Elapsed frame time: %f", static_cast<float>(dt));
float yawCommand = normalizeAngle(currentYaw - targetYaw); float yawCommand = normalizeAngle(currentYaw - targetYaw);
yawCommand = 0.0; // HACK
// check dir we should go // check dir we should go
ROS_INFO("yaw command: %f", yawCommand); ROS_INFO("yaw command: %f", yawCommand);
// convert both target and current position, and move between // convert both target and current position, and move between
std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos); // std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos); // 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) // 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], // using commanded velocity instead of actual
targetPosMapped[1] - currentPosMapped[1], static_cast<float>(((targetPos[0] - currentPos[0])*ksg + (targetVel[0] - velocityCommand[0])*kdg)*speedScalarParam),
targetPosMapped[2] - currentPosMapped[2] 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 // go to position
ROS_INFO("calling move command w %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]); // ROS_INFO("calling move command %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
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 updateRate.sleep();
ros::spinOnce();
// check if position call was correct in terms of distance TODO also do this process for the rotation matrix // check if position call was correct in terms of distance TODO also do this process for the rotation matrix
// --------------------------------- // ---------------------------------
std::vector<float> calCheckPosition // std::vector<float> calCheckPosition
{ // {
static_cast<float>(ex_pos.translation.x), // static_cast<float>(ex_pos.translation.x),
static_cast<float>(ex_pos.translation.y), // static_cast<float>(ex_pos.translation.y),
static_cast<float>(ex_pos.translation.z) // static_cast<float>(ex_pos.translation.z)
}; // };
// what the distance actually was in ref coords // // what the distance actually was in ref coords
float refDist = eucDistance(currentPos, calCheckPosition); // float refDist = eucDistance(currentPos, calCheckPosition);
// what the distance was in command coords // // what the distance was in command coords
float cmdDist = Magnitude3(moveCommand); // float cmdDist = Magnitude3(force);
if (cmdDist > stdMoveDist) // if (cmdDist > stdMoveDist)
{ // {
cmdDist = stdMoveDist; // cmdDist = stdMoveDist;
} // }
// if movecommand, mapped to coords, does not match up with difference in position, scream // updateMagnitudeScaler(refDist/cmdDist);
updateMagnitudeScaler(refDist/cmdDist);
// ---------------------------------
updateRate.sleep();
} }
} }
else else
...@@ -502,11 +573,21 @@ bool runPositionMission() ...@@ -502,11 +573,21 @@ bool runPositionMission()
int numPositions; int numPositions;
ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions); ros::param::get("/robot_list/robot_" + to_string(droneId) + "/numPositions", numPositions);
int currentPositionIndex = 0; // will increment 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; positionMissionFlag = true;
while (positionMissionFlag && ros::ok()) 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 std::vector<float> currentPos
{ {
static_cast<float>(ex_pos.translation.x), static_cast<float>(ex_pos.translation.x),
...@@ -563,46 +644,37 @@ bool runPositionMission() ...@@ -563,46 +644,37 @@ bool runPositionMission()
ROS_INFO("calling move command w %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]); ROS_INFO("calling move command w %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand); relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
// let topics update updateRate.sleep();
ros::spinOnce();
// check if position call was correct in terms of distance TODO also do this process for the rotation matrix // check if position call was correct in terms of distance TODO also do this process for the rotation matrix
// --------------------------------- // ---------------------------------
std::vector<float> calCheckPosition // std::vector<float> calCheckPosition
{ // {
static_cast<float>(ex_pos.translation.x), // static_cast<float>(ex_pos.translation.x),
static_cast<float>(ex_pos.translation.y), // static_cast<float>(ex_pos.translation.y),
static_cast<float>(ex_pos.translation.z) // static_cast<float>(ex_pos.translation.z)
}; // };
// what the distance actually was in ref coords // // what the distance actually was in ref coords
float refDist = eucDistance(currentPos, calCheckPosition); // float refDist = eucDistance(currentPos, calCheckPosition);
// what the distance was in command coords // // what the distance was in command coords
float cmdDist = Magnitude3(moveCommand); // float cmdDist = Magnitude3(moveCommand);
if (cmdDist > stdMoveDist) // if (cmdDist > stdMoveDist)
{ // {
cmdDist = stdMoveDist; // cmdDist = stdMoveDist;
} // }
// if movecommand, mapped to coords, does not match up with difference in position, scream // // if movecommand, mapped to coords, does not match up with difference in position, scream
//
updateMagnitudeScaler(refDist/cmdDist); // updateMagnitudeScaler(refDist/cmdDist);
// --------------------------------- // // ---------------------------------
// // check if we're close to position
// check if we're close to position // float checkDist = eucDistance(currentPos, targetPos);
float checkDist = eucDistance(currentPos, targetPos); // if (checkDist <= 0.4) //&& abs(currentYaw - targetYaw) <= 0.1) // close ish
if (checkDist <= 0.4) //&& abs(currentYaw - targetYaw) <= 0.1) // close ish // {
{ // currentPositionIndex += 1; // go to next position
currentPositionIndex += 1; // go to next position // }
} // if (currentPositionIndex >= numPositions)
// {
if (currentPositionIndex >= numPositions) // positionMissionFlag = false;
{ // }
// leave
positionMissionFlag = false;
}
// ROS_INFO("distance to first position: %f", firstPosDist);
updateRate.sleep();
} }
} }
...@@ -628,7 +700,7 @@ void setWaypointDefaults(WayPointSettings* wp) ...@@ -628,7 +700,7 @@ void setWaypointDefaults(WayPointSettings* wp)
void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask) void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask)
{ {
waypointTask.velocity_range = 2; waypointTask.velocity_range = baseSpeedParam;
waypointTask.idle_velocity = baseSpeedParam; waypointTask.idle_velocity = baseSpeedParam;
waypointTask.action_on_finish = dji_osdk_ros::MissionWaypointTask::FINISH_NO_ACTION; waypointTask.action_on_finish = dji_osdk_ros::MissionWaypointTask::FINISH_NO_ACTION;
waypointTask.mission_exec_times = 1; waypointTask.mission_exec_times = 1;
...@@ -767,19 +839,30 @@ bool relativePosition(float x, float y, float z, float yaw) ...@@ -767,19 +839,30 @@ bool relativePosition(float x, float y, float z, float yaw)
float totalDist = Magnitude3(posVec); float totalDist = Magnitude3(posVec);
// normalize maximum distance to a parameter, say 1m steps so we can do position updates as we go // 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 speed = maxSpeedParam;
std::vector<float> normPos = normalizeVector(posVec); totalTimeMs = controlLoopTime*1000.0;
posVec[0] = normPos[0]*stdMoveDist; }
posVec[1] = normPos[1]*stdMoveDist; else
posVec[2] = normPos[2]*stdMoveDist; {
totalDist = stdMoveDist; 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> velDir = normalizeVector(posVec);
std::vector<float> velVec = {velDir[0]*speed, velDir[1]*speed, velDir[2]*speed}; 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) ...@@ -808,7 +891,7 @@ bool relativePosition(float x, float y, float z, float yaw)
joystickParams.yaw = 0.0; 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"); ROS_INFO("---------------\n");
flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL; 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) ...@@ -816,7 +899,6 @@ bool relativePosition(float x, float y, float z, float yaw)
flightTaskControl.request.velocityControlTimeMs = totalTimeMs; flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
flightTaskControl.request.posThresholdInM = 0.1; flightTaskControl.request.posThresholdInM = 0.1;
flightTaskControl.request.yawThresholdInDeg = 1; flightTaskControl.request.yawThresholdInDeg = 1;
flight_control_client.call(flightTaskControl); // actually waits for drone to "reach" position flight_control_client.call(flightTaskControl); // actually waits for drone to "reach" position
return flightTaskControl.response.result; return flightTaskControl.response.result;
...@@ -858,11 +940,6 @@ void updateMagnitudeScaler(float newValue) ...@@ -858,11 +940,6 @@ void updateMagnitudeScaler(float newValue)
sumer += calibrationWindowValues[i]; sumer += calibrationWindowValues[i];
} }
poseMagnitudeScaler = sumer/calibrationWindowValues.size(); 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) 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 ...@@ -975,7 +1052,7 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec) std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec)
{ {
// move a bit and return diff in position // move a bit and return diff in position
ros::spinOnce(); // let callbacks update //ros::spinOnce(); // let callbacks update
geometry_msgs::Transform preT = ex_pos; geometry_msgs::Transform preT = ex_pos;
relativePosition(calibrationVec[0], calibrationVec[1], calibrationVec[2], 0.0); relativePosition(calibrationVec[0], calibrationVec[1], calibrationVec[2], 0.0);
ros::spinOnce(); // let callbacks update ros::spinOnce(); // let callbacks update
...@@ -988,6 +1065,7 @@ std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec) ...@@ -988,6 +1065,7 @@ std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec)
}; };
// go back // go back
relativePosition(-calibrationVec[0], -calibrationVec[1], -calibrationVec[2], 0.0); 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; return deltaPosition;
} }
...@@ -999,13 +1077,22 @@ int main(int argc, char** argv) ...@@ -999,13 +1077,22 @@ int main(int argc, char** argv)
ros::param::get("base_speed", baseSpeedParam); ros::param::get("base_speed", baseSpeedParam);
ros::param::get("min_speed", minSpeedParam); 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("max_yaw_speed", maxYawSpeedParam);
ros::param::get("speed_scalar", speedScalarParam); ros::param::get("speed_scalar", speedScalarParam);
ros::param::get("drone_id", droneId); ros::param::get("drone_id", droneId);
ros::param::get("position_playback", positionPlaybackParam); ros::param::get("position_playback", positionPlaybackParam);
ros::param::get("std_move_dist", stdMoveDist); ros::param::get("loop_rate", updateRateParam);
// ROS_INFO("base speed set to %f", baseSpeedParam); 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 // ROS stuff
// obtain control authority? // 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