Skip to content

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

qualisys further changes

parent 7b08a026
Hide whitespace changes
Inline Side-by-side
Showing with 269 additions and 106 deletions
Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h
View file @ 0f61d212
......@@ -71,6 +71,7 @@ typedef struct ServiceAck
float eucDistance(std::vector<float> a, std::vector<float> b);
float normalizeAngle(float angle);
std::vector<std::vector<float>> rotateAxisAngle(std::vector<float> u, float angleRad);
float dotProduct(std::vector<float> vect_A, std::vector<float> vect_B);
std::vector<float> crossProduct(std::vector<float> vect_A, std::vector<float> vect_B);
......
Onboard-SDK-ROS/launch/local_test.launch
View file @ 0f61d212
......@@ -11,18 +11,27 @@
<param name="enc_key" type="string" value="e180b993ca8365653437fbafe7211ba040386d77c3c87627882857a11bd8efbd"/>
<param name="use_broadcast" type="bool" value="false"/>
</node>
<!-- <param name="min_yaw_speed" type="double" value="0.0"/>
<param name="max_yaw_speed" type="double" value="15.0"/> -->
<param name="base_speed" type="double" value="0.5"/>
<param name="min_speed" type="double" value="0.1"/>
<param name="speed_scalar" type="double" value="0.01"/>
<!-- max yaw rate in deg/s -->
<param name="max_yaw_speed" type="double" value="30.0"/>
<param name="position_playback" type="bool" value="false"/>
<param name="drone_id" type="int" value="0"/>
<param name="camera_type" type="int" value="1"/>
<!-- <param name="angle_offset" type="double" value="-33"/> -->
<node pkg="dji_osdk_ros" type="bus_driver" name="bus_driver" output="screen"/>
<node pkg="dji_osdk_ros" type="poseFeedbackConversion.py" name="poseFeedbackConversion" output="screen">
<!-- <node pkg="dji_osdk_ros" type="poseFeedbackConversion.py" name="poseFeedbackConversion" output="screen">
<rosparam command="load" param="angleOffset">
-33.0
0.0
</rosparam>
</node> -->
<node pkg="dji_osdk_ros" type="qualisysFeedback.py" name="qualisysFeedback" output="screen">
<param name="trackName" type="string" value="M100_CarbonFiber"/>
</node>
<node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros">
......
Onboard-SDK-ROS/scripts/poseFeedbackConversion.py
View file @ 0f61d212
......@@ -3,6 +3,7 @@
import rospy
import numpy as np
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import QuaternionStamped
from dji_osdk_ros.msg import VOPosition
......@@ -34,7 +35,9 @@ def point_rotation(origin, pt, ang):
def rotCallback(data):
global quat
quat = data.quaternion
quat = data.orientation;
# # FINE ill DO it MYself all ALONE
def posCallback(data):
global homeSetFlag
......@@ -51,21 +54,16 @@ def posCallback(data):
rotation = Quaternion()
# rotate the xy plane by angle
# dataPointSpun = point_rotation([0, 0], [data.x, data.y], angleOffset)
homePointMeters = lat_lon2meters([homePoint.latitude, homePoint.longitude])
currentPointMeters = lat_lon2meters([data.latitude, data.longitude])
currentOffset = [currentPointMeters[0] - homePointMeters[0], currentPointMeters[1] - homePointMeters[1]]
# angleOffset = -33.0
# rospy.loginfo(angleOffset)
# rospy.loginfo(currentOffset)
angleOffset = rospy.get_param("~angleOffset")
currentRotatedOffset = point_rotation([0,0], currentOffset, angleOffset)
# rospy.loginfo(currentRotatedOffset)
# performing dark magics
translation.x = currentRotatedOffset[1]
translation.y = currentRotatedOffset[0]
translation.z = data.altitude - homePoint.altitude
......@@ -73,19 +71,11 @@ def posCallback(data):
transform.translation = translation
transform.rotation = quat
# rot = Rotation.from_euler('xyz', [0, 0, 0], degrees = True) # be not rotated pls
# zeroQuaternion = rot.as_quat()
# rotation.x = zeroQuaternion[0]
# rotation.y = zeroQuaternion[1]
# rotation.z = zeroQuaternion[2]
# rotation.w = zeroQuaternion[3]
def main():
global transform
rospy.init_node("poseFeedbackConversion")
rospy.Subscriber("dji_osdk_ros/gps_position", NavSatFix, posCallback, queue_size = 1)
rospy.Subscriber("dji_osdk_ros/attitude", QuaternionStamped, rotCallback, queue_size = 1)
rospy.Subscriber("dji_osdk_ros/imu", Imu, rotCallback, queue_size = 1)
posePublisher = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
while not rospy.is_shutdown():
posePublisher.publish(transform)
......
Onboard-SDK-ROS/scripts/qualisysFeedback.py 0 → 100755
View file @ 0f61d212
#!/usr/bin/python3
import rospy
import numpy as np
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Transform
transform = Transform()
def callback(data):
# save the pose date in transform
transform.rotation = data.pose.orientation
# we must remapp
# transform.translation = data.pose.position
transform.translation.x = data.pose.position.z
transform.translation.z = data.pose.position.y
transform.translation.y = data.pose.position.x
def main():
global transform
rospy.init_node("qualisysFeedback")
name = rospy.get_param("~trackName")
rospy.loginfo("looking for track of name " + str(name) + " ...")
rospy.Subscriber("/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
pub = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
while not rospy.is_shutdown():
pub.publish(transform)
if __name__ == '__main__':
main()
Onboard-SDK-ROS/scripts/tools.py
View file @ 0f61d212
......@@ -12,3 +12,10 @@ def lat_lon2meters(latlon_point):
transformer = Transformer.from_crs(4326, 3857)
meters_point = transformer.transform(latlon_point[0], latlon_point[1])
return meters_point
def normalizeAngle(angle):
newAngle = angle%360
newAngle = (newAngle + 360)%360
if(newAngle >= 180):
newAngle -= 360
return newAngle
Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py
View file @ 0f61d212
......@@ -58,7 +58,7 @@ def generateWaypoints():
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)
waypointHeading = (45.0*i)%360.0
# waypoints
waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseWaypointAltitude + d*waypointAltitudeIncrement, waypointHeading]})
......
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
View file @ 0f61d212
......@@ -25,6 +25,7 @@
* SOFTWARE.
*
*/
#include <math.h> /* atan2 */
#include <dji_osdk_ros/bus_driver.h>
#include <dji_osdk_ros/FlightTaskControl.h>
#include <dji_osdk_ros/ObtainControlAuthority.h>
......@@ -53,10 +54,14 @@ 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)
float externalYaw; // yaw as measured by pos ref
float externalPitch; // pitch as measured by pos ref
float externalRoll; // roll as measured by pos ref
bool gps_home_flag = false;
bool pos_home_flag = false;
bool positionMissionFlag = false;
bool waypointMissionFlag = false;
bool positionPlaybackParam = false;
ros::Subscriber gps_pos_subscriber;
ros::Subscriber pos_feedback_subscriber;
int droneId; // stupid but will work for now
......@@ -74,6 +79,8 @@ long double initialPositionDistance; // distance to first position
double baseSpeedParam;
double minSpeedParam;
double speedScalarParam;
double minYawSpeedParam;
double maxYawSpeedParam;
std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
float poseMagnitudeScaler; // need to run calibration to get
......@@ -92,6 +99,17 @@ long double toRadians(const long double degree)
return (one_deg * degree);
}
float normalizeAngle(float angle)
{
float newAngle = std::fmod(angle, (2.0*M_PI));
newAngle = std::fmod((newAngle + (2.0*M_PI)), (2.0*M_PI));
if(newAngle >= M_PI)
{
newAngle -= (2.0*M_PI);
}
return newAngle;
}
float eucDistance(std::vector<float> a, std::vector<float> b)
{
// do the math bb
......@@ -303,6 +321,12 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
{
ex_pos = *msg; // alllllways load the message
// FINE ill DO it MYself all ALONE
externalYaw = atan2(2.0*(ex_pos.rotation.x*ex_pos.rotation.y + ex_pos.rotation.z*ex_pos.rotation.w), 1 - 2.0*(ex_pos.rotation.y*ex_pos.rotation.y + ex_pos.rotation.z*ex_pos.rotation.z));
externalPitch = asin(2.0*(ex_pos.rotation.x*ex_pos.rotation.z - ex_pos.rotation.w*ex_pos.rotation.y));
externalRoll = atan2(2.0*(ex_pos.rotation.x*ex_pos.rotation.w + ex_pos.rotation.y*ex_pos.rotation.z), 1 - 2.0*(ex_pos.rotation.z*ex_pos.rotation.z + ex_pos.rotation.w*ex_pos.rotation.w));
// ROS_INFO("current (roll pitch yaw): (%f, %f, %f)", externalRoll, externalPitch, externalYaw);
if (!positionMissionFlag)
{
return; // peace out if not running mission
......@@ -375,84 +399,168 @@ bool runWaypointMission()
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
positionMissionFlag = true;
while (positionMissionFlag && ros::ok())
if (positionPlaybackParam)
{
// print distance to first position
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> tempPos;
ros::param::get("/robot_list/robot_" + to_string(droneId) + "/positions/position_" + to_string(currentPositionIndex), tempPos);
// pull out x,y,z
std::vector<float> targetPos
{
tempPos[1], // 0th index is the index of position, facepalm
tempPos[2],
tempPos[3]
};
// may need to rotate this yaw to match coordinate system
float targetYaw = (M_PI/180.0)*tempPos[4];
Eigen::Quaternion<float> currentRotation = {float(ex_pos.rotation.w), float(ex_pos.rotation.x), float(ex_pos.rotation.y), float(ex_pos.rotation.z)};
auto currentRotationEuler = currentRotation.toRotationMatrix().eulerAngles(0, 1, 2);
// TODO need angle mapping right here!!!!
float currentYaw = currentRotationEuler[2];
ROS_INFO("target yaw: %f", targetYaw);
ROS_INFO("current yaw: %f", currentYaw);
float yawCommand = (180.0/M_PI)*(targetYaw - currentYaw);
// map to our coordinate system
if (!rotationMatrixCalibratedFlag) // could also do this before loop?
// replaying a capture from reference WIP
// ros::Rate updateRate(20); // update period for topics/commands
// while (positionMissionFlag && ros::ok())
// {
// // print distance to first position
// 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> tempPos;
// ros::param::get("/robot_list/robot_" + to_string(droneId) + "/positions/position_" + to_string(currentPositionIndex), tempPos);
// // pull out x,y,z
// std::vector<float> targetPos
// {
// tempPos[1], // 0th index is the index of position, facepalm
// tempPos[2],
// tempPos[3]
// };
//
// if (!rotationMatrixCalibratedFlag) // could also do this before loop?
// {
// // run rotation matrix calc
// calibratePositionReference();
// rotationMatrixCalibratedFlag = true;
// }
//
// // may need to rotate this yaw to match coordinate system
// // Eigen::Quaterniond currentRotation(float(ex_pos.rotation.w), float(ex_pos.rotation.x), float(ex_pos.rotation.y), float(ex_pos.rotation.z));
// // auto currentRotationEuler = currentRotation.toRotationMatrix().eulerAngles(0, 1, 2);
// float targetYaw = applyRotationAngle(normalizeAngle((M_PI/180.0)*tempPos[4]));
// float currentYaw = externalYaw;
// 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
// std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
// std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// // map yaw command (in deg) to rotation offset (in rads)
// // float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
//
// std::vector<float> moveCommand
// {
// targetPosMapped[0] - currentPosMapped[0],
// targetPosMapped[1] - currentPosMapped[1],
// targetPosMapped[2] - currentPosMapped[2]
// };
//
// // go to position
// relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
//
// ros::spinOnce();
//
// // check if we're close to position
// float checkDist = eucDistance(currentPos, targetPos);
// if (checkDist <= 0.2 && abs(currentYaw - targetYaw) <= 0.1) // close ish
// {
// currentPositionIndex += 1; // go to next position
// }
//
// if (currentPositionIndex >= numPositions)
// {
// // leave
// positionMissionFlag = false;
// }
// // ROS_INFO("distance to first position: %f", firstPosDist);
//
//
// updateRate.sleep();
// }
}
else
{
// 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(20); // update period for topics/commands
positionMissionFlag = true;
while (positionMissionFlag && ros::ok())
{
// run rotation matrix calc
calibratePositionReference();
rotationMatrixCalibratedFlag = true;
}
// print distance to first position
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> tempPos;
ros::param::get("/robot_list/robot_" + to_string(droneId) + "/positions/position_" + to_string(currentPositionIndex), tempPos);
// pull out x,y,z
std::vector<float> targetPos
{
tempPos[1], // 0th index is the index of position, facepalm
tempPos[2],
tempPos[3]
};
// convert both target and current position, and move between
std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// 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
if (!rotationMatrixCalibratedFlag) // could also do this before loop?
{
// run rotation matrix calc
calibratePositionReference();
rotationMatrixCalibratedFlag = true;
}
std::vector<float> moveCommand
{
targetPosMapped[0] - currentPosMapped[0],
targetPosMapped[1] - currentPosMapped[1],
targetPosMapped[2] - currentPosMapped[2]
};
// may need to rotate this yaw to match coordinate system
// Eigen::Quaterniond currentRotation(float(ex_pos.rotation.w), float(ex_pos.rotation.x), float(ex_pos.rotation.y), float(ex_pos.rotation.z));
// auto currentRotationEuler = currentRotation.toRotationMatrix().eulerAngles(0, 1, 2);
float targetYaw = applyRotationAngle(normalizeAngle((M_PI/180.0)*tempPos[4]));
float currentYaw = externalYaw;
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
std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// map yaw command (in deg) to rotation offset (in rads)
// float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
std::vector<float> moveCommand
{
targetPosMapped[0] - currentPosMapped[0],
targetPosMapped[1] - currentPosMapped[1],
targetPosMapped[2] - currentPosMapped[2]
};
// go to position
relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
// go to position
relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
ros::spinOnce();
ros::spinOnce();
// check if we're close to position
float checkDist = eucDistance(currentPos, targetPos);
if (checkDist <= 0.2) // close ish
{
currentPositionIndex += 1; // go to next position
}
// check if we're close to position
float checkDist = eucDistance(currentPos, targetPos);
if (checkDist <= 0.2 && abs(currentYaw - targetYaw) <= 0.1) // close ish
{
currentPositionIndex += 1; // go to next position
}
if (currentPositionIndex >= numPositions)
{
// leave
positionMissionFlag = false;
}
// ROS_INFO("distance to first position: %f", firstPosDist);
if (currentPositionIndex >= numPositions)
{
// leave
positionMissionFlag = false;
}
// ROS_INFO("distance to first position: %f", firstPosDist);
updateRate.sleep();
updateRate.sleep();
}
}
return true;
}
......@@ -593,17 +701,16 @@ bool land()
return flightTaskControl.response.result;
}
float applyRotationAngle(float targetYaw)
// converts local yaw in rads to mapped yaw in rads
float applyRotationAngle(float localYaw)
{
// make unit vector out of yaw, rotate, dot with pos x, get angle
const float sinA = sinf( (M_PI/180.0)*targetYaw );
const float cosA = cosf( (M_PI/180.0)*targetYaw );
std::vector<float> targetDir = {cosA, sinA, 0.0};
std::vector<float> rotatedDir = MatrixMultiply(poseRotationMatrix, targetDir);
// convert back into angle
std::vector<float> referenceAxis = {1.0, 0.0, 0.0};
float rotatedYaw = dotProduct(referenceAxis, rotatedDir);
return (180.0/M_PI)*acosf(rotatedYaw);
if (!rotationMatrixCalibratedFlag) // could also do this before loop?
{
// run rotation matrix calc
calibratePositionReference();
rotationMatrixCalibratedFlag = true;
}
return localYaw + poseRotationRadians;
}
bool relativePosition(float x, float y, float z, float yaw)
......@@ -613,16 +720,40 @@ bool relativePosition(float x, float y, float z, float yaw)
float totalDist = Magnitude3(posVec);
float speed = min(speedScalarParam*pow(totalDist,2) + minSpeedParam, baseSpeedParam);
// float yawSpeed = max(min(abs(yaw), static_cast<float>(maxYawSpeedParam)), static_cast<float>(minYawSpeedParam));
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};
dji_osdk_ros::FlightTaskControl flightTaskControl;
dji_osdk_ros::JoystickParams joystickParams;
// float sentYaw = ((abs(yaw)/yaw)*yawSpeed)/(totalTimeMs/1000.0);
joystickParams.x = velVec[0];
joystickParams.y = velVec[1];
joystickParams.z = velVec[2];
joystickParams.yaw = yaw/(totalTimeMs/1000.0); // assuming this is in rads/sec??
if (abs(yaw) >= 0.01)
{
float tempYawRate = (180.0/M_PI)*yaw/(totalTimeMs/1000.0);
if (abs(tempYawRate) > maxYawSpeedParam)
{
tempYawRate = (tempYawRate/abs(tempYawRate))*maxYawSpeedParam;
// if distance is small, let yaw dictate duration
if (totalDist <= 0.1)
{
totalTimeMs = (abs(yaw)/tempYawRate)*1000.0;
}
}
ROS_INFO("yaw rate: %f over %f sec", tempYawRate, (totalTimeMs/1000.0));
ROS_INFO("---------------\n");
joystickParams.yaw = tempYawRate;
}
else
{
joystickParams.yaw = 0.0;
}
flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL;
flightTaskControl.request.joystickCommand = joystickParams;
flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
......@@ -689,14 +820,6 @@ void calibratePositionReference()
std::vector<float> checkPosition = applyRotationMatrixScaling(requestedPosition);
relativePosition(reversePosition[0], reversePosition[1], reversePosition[2], 0.0); // go to requested position in corrected frame
ros::spinOnce();
// write rotation and scale to parameters
// for (int i = 0; i < poseRotationMatrix.size(); i++) {
// for (int j = 0; j < poseRotationMatrix[0].size(); j++) {
// ros::param::set("poseRotationMatrix/val_" + to_string(i) + to_string(j), poseRotationMatrix[i][j]);
// }
// }
// ros::param::set("poseMagnitudeScaler", poseMagnitudeScaler);
}
std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec)
......@@ -789,8 +912,11 @@ 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_yaw_speed", maxYawSpeedParam);
ros::param::get("speed_scalar", speedScalarParam);
ros::param::get("drone_id", droneId);
ros::param::get("position_playback", positionPlaybackParam);
// ROS_INFO("base speed set to %f", baseSpeedParam);
// ROS stuff
......
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