Skip to content

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

cube testing updates

parent 0f61d212
Hide whitespace changes
Inline Side-by-side
Showing with 261 additions and 179 deletions
Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h
View file @ 1f96d6f8
......@@ -123,14 +123,18 @@ bool stopMotion();
bool goHome();
void calibratePositionReference();
void calibratePositionReference(std::vector<float> requestedPosition, std::vector<float> actualPosition);
void updateMagnitudeScaler(float newValue);
std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec);
bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res);
std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec);
void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg);
void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg);
#endif // MISSION_NODE_H
Onboard-SDK-ROS/launch/local_test.launch
View file @ 1f96d6f8
......@@ -16,23 +16,24 @@
<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"/>
<param name="speed_scalar" type="double" value="0.5"/>
<!-- 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"/> -->
<param name="std_move_dist" type="double" value="1.0"/>
<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">
<rosparam command="load" param="angleOffset">
0.0
</rosparam>
</node> -->
<node pkg="dji_osdk_ros" type="qualisysFeedback.py" name="qualisysFeedback" output="screen">
<param name="trackName" type="string" value="M100_CarbonFiber"/>
<!-- gps feedback node version -->
<node pkg="dji_osdk_ros" type="poseFeedbackConversion.py" name="poseFeedbackConversion" output="screen">
<rosparam command="load" param="angleOffset"> 0.0 </rosparam>
<rosparam command="load" param="anchorPoint"> [37.196986, -80.578257, 100.0] </rosparam>
</node>
<!-- qualisys feedback node version -->
<!-- <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">
<rosparam command="load" param="anchorPoint"> [37.196986, -80.578257] </rosparam>
......
Onboard-SDK-ROS/scripts/poseFeedbackConversion.py
View file @ 1f96d6f8
......@@ -15,10 +15,11 @@ from tools import lat_lon2meters, meters2lat_lon
import math
homeSetFlag = False
# homePoint = PointStamped()
homePoint = NavSatFix()
transform = Transform()
quat = Quaternion()
translation = Vector3()
posePublisher = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
angleOffset = 0.0
def point_rotation(origin, pt, ang):
......@@ -35,51 +36,34 @@ def point_rotation(origin, pt, ang):
def rotCallback(data):
global quat
quat = data.orientation;
# # FINE ill DO it MYself all ALONE
quat = data.quaternion;
def posCallback(data):
global homeSetFlag
global homePoint
global transform
if not homeSetFlag: # mildly hacky, depends on position format we're using for feedback. might build into bus driver and pub from there
# set up home point in yee pee es
homePoint = data
homeSetFlag = True
rospy.loginfo("setting home point...")
global quat
# convert and compare against home point
translation = Vector3()
rotation = Quaternion()
# rotate the xy plane by angle
homePointMeters = lat_lon2meters([homePoint.latitude, homePoint.longitude])
homePoint = rospy.get_param("~anchorPoint")
homePointMeters = lat_lon2meters([homePoint[0], homePoint[1]])
currentPointMeters = lat_lon2meters([data.latitude, data.longitude])
currentOffset = [currentPointMeters[0] - homePointMeters[0], currentPointMeters[1] - homePointMeters[1]]
angleOffset = rospy.get_param("~angleOffset")
currentRotatedOffset = point_rotation([0,0], currentOffset, angleOffset)
# performing dark magics
translation = Vector3()
translation.x = currentRotatedOffset[1]
translation.y = currentRotatedOffset[0]
translation.z = data.altitude - homePoint.altitude
translation.z = data.altitude - homePoint[2]
transform.translation = translation
transform.rotation = quat
posePublisher.publish(transform)
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/imu", Imu, rotCallback, queue_size = 1)
posePublisher = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
while not rospy.is_shutdown():
posePublisher.publish(transform)
rospy.Subscriber("dji_osdk_ros/attitude", QuaternionStamped, rotCallback, queue_size = 1)
rospy.spin()
# get anchor point from params
if __name__ == '__main__':
main()
Onboard-SDK-ROS/scripts/qualisysFeedback.py
View file @ 1f96d6f8
......@@ -10,21 +10,21 @@ 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 = data.pose.position
transform.translation.x = data.pose.position.x
transform.translation.y = 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")
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("/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
pub = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
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)
if __name__ == '__main__':
main()
Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py
View file @ 1f96d6f8
......@@ -20,9 +20,9 @@ def generateWaypoints():
# musical drones trajectory is convient test case, but general path is possible
# ring sizes
big = 10
med = 5
sml = 3
big = 5
med = 3
sml = 1
apMeters = lat_lon2meters(anchorPoint)
......
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
View file @ 1f96d6f8
/** @file mission_node.cpp
* @version 4.0
* @date June 2020
......@@ -54,9 +55,22 @@ 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
geometry_msgs::Transform 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
float trackingYaw; // yaw to track
float trackingPitch; // pitch to track
float trackingRoll; // roll to track
int calibrationWindow = 5; // the last _ position calls that are included in coordinate system calibration
std::vector<float> calibrationWindowValues;
float stdMoveDist;
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
float poseRotationRadians;
bool rotationMatrixCalibratedFlag = false;
bool gps_home_flag = false;
bool pos_home_flag = false;
bool positionMissionFlag = false;
......@@ -64,6 +78,7 @@ bool waypointMissionFlag = false;
bool positionPlaybackParam = false;
ros::Subscriber gps_pos_subscriber;
ros::Subscriber pos_feedback_subscriber;
ros::Subscriber pos_tracking_subscriber;
int droneId; // stupid but will work for now
int waypointMissionUpdateCounter = 0; // counts iterations before we check waypoints again
......@@ -82,11 +97,6 @@ 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
float poseRotationRadians;
bool rotationMatrixCalibratedFlag = false;
// Utility function for
// converting degrees to radians
long double toRadians(const long double degree)
......@@ -322,17 +332,19 @@ 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));
feedbackYaw = 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));
feedbackPitch = asin(2.0*(ex_pos.rotation.x*ex_pos.rotation.z - ex_pos.rotation.w*ex_pos.rotation.y));
feedbackRoll = 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
}
void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg)
{
track_pos = *msg;
// do loopin' stuff to get close to waypoints
// 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));
}
bool setupWaypointMission(int responseTimeout)
......@@ -402,80 +414,87 @@ bool runPositionMission()
if (positionPlaybackParam)
{
// 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();
// }
// executing a set of positions in reference
ros::Rate updateRate(20); // update period for topics/commands
positionMissionFlag = true;
while (positionMissionFlag && ros::ok())
{
std::vector<float> currentPos
{
static_cast<float>(ex_pos.translation.x),
static_cast<float>(ex_pos.translation.y),
static_cast<float>(ex_pos.translation.z)
};
std::vector<float> targetPos
{
static_cast<float>(track_pos.translation.x),
static_cast<float>(track_pos.translation.y),
static_cast<float>(track_pos.translation.z)
};
if (!rotationMatrixCalibratedFlag) // could also do this before loop?
{
// run rotation matrix calc
std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
calibratePositionReference(requestedPosition, actualPosition);
rotationMatrixCalibratedFlag = true;
}
float targetYaw = applyRotationAngle(trackingYaw); // might need to use different axis here, bc rotations
float currentYaw = feedbackYaw;
ROS_INFO("current postion %f, %f, %f", currentPos[0], currentPos[1], currentPos[2]);
ROS_INFO("target position %f, %f, %f", targetPos[0], targetPos[1], targetPos[2]);
ROS_INFO("target yaw: %f", targetYaw);
ROS_INFO("current yaw: %f", currentYaw);
float yawCommand = normalizeAngle(currentYaw - targetYaw);
// check dir we should go
ROS_INFO("yaw command: %f", yawCommand);
// convert both target and current position, and move between
std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// map yaw command (in deg) to rotation offset (in rads)
std::vector<float> moveCommand
{
targetPosMapped[0] - currentPosMapped[0],
targetPosMapped[1] - currentPosMapped[1],
targetPosMapped[2] - currentPosMapped[2]
};
// 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);
// let topics update
ros::spinOnce();
// 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();
}
}
else
{
......@@ -488,7 +507,6 @@ bool runPositionMission()
while (positionMissionFlag && ros::ok())
{
// print distance to first position
std::vector<float> currentPos
{
static_cast<float>(ex_pos.translation.x),
......@@ -508,15 +526,17 @@ bool runPositionMission()
if (!rotationMatrixCalibratedFlag) // could also do this before loop?
{
// run rotation matrix calc
calibratePositionReference();
std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
calibratePositionReference(requestedPosition, actualPosition);
rotationMatrixCalibratedFlag = true;
}
// may need to rotate this yaw to match coordinate system
// 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;
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);
......@@ -525,8 +545,10 @@ bool runPositionMission()
ROS_INFO("yaw command: %f", yawCommand);
// convert both target and current position, and move between
// ROS_INFO("applying rotation matrix scaling...");
std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// ROS_INFO("successfully applied rotation matrix");
// map yaw command (in deg) to rotation offset (in rads)
// float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
......@@ -538,13 +560,36 @@ bool runPositionMission()
};
// 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);
// let topics update
ros::spinOnce();
// 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.2 && abs(currentYaw - targetYaw) <= 0.1) // close ish
if (checkDist <= 0.4) //&& abs(currentYaw - targetYaw) <= 0.1) // close ish
{
currentPositionIndex += 1; // go to next position
}
......@@ -707,7 +752,9 @@ float applyRotationAngle(float localYaw)
if (!rotationMatrixCalibratedFlag) // could also do this before loop?
{
// run rotation matrix calc
calibratePositionReference();
std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
calibratePositionReference(requestedPosition, actualPosition);
rotationMatrixCalibratedFlag = true;
}
return localYaw + poseRotationRadians;
......@@ -718,9 +765,19 @@ bool relativePosition(float x, float y, float z, float yaw)
// convert relative position to velocity command
std::vector<float> posVec {x, y, z};
float totalDist = Magnitude3(posVec);
float speed = min(speedScalarParam*pow(totalDist,2) + minSpeedParam, baseSpeedParam);
// float yawSpeed = max(min(abs(yaw), static_cast<float>(maxYawSpeedParam)), static_cast<float>(minYawSpeedParam));
// normalize maximum distance to a parameter, say 1m steps so we can do position updates as we go
if (totalDist > stdMoveDist)
{
// normalize the move to be of std length
std::vector<float> normPos = normalizeVector(posVec);
posVec[0] = normPos[0]*stdMoveDist;
posVec[1] = normPos[1]*stdMoveDist;
posVec[2] = normPos[2]*stdMoveDist;
totalDist = stdMoveDist;
}
float 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);
......@@ -729,8 +786,6 @@ bool relativePosition(float x, float y, float z, float yaw)
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];
......@@ -746,14 +801,16 @@ bool relativePosition(float x, float y, float z, float yaw)
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;
}
ROS_INFO("yaw rate: %f | speed: %f, over %f sec", joystickParams.yaw, speed, (totalTimeMs/1000.0));
ROS_INFO("---------------\n");
flightTaskControl.request.task = dji_osdk_ros::FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL;
flightTaskControl.request.joystickCommand = joystickParams;
flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
......@@ -788,23 +845,28 @@ bool goHome()
return true;
}
void calibratePositionReference()
{
// call move and check what pos ref says
ros::spinOnce(); // let callbacks update
geometry_msgs::Transform preT = ex_pos;
relativePosition(0.5, 0.0, 0.0, 0.0);
ros::spinOnce(); // let callbacks update
geometry_msgs::Transform postT = ex_pos;
std::vector<float> requestedPosition {0.5, 0.0, 0.0}; // what we asked for
std::vector<float> reversePosition {-0.5, 0.0, 0.0}; // what we asked for
std::vector<float> actualPosition // what we got
void updateMagnitudeScaler(float newValue)
{
// remove the last value and insert new one
calibrationWindowValues.pop_back();
auto firstIndex = calibrationWindowValues.begin();
calibrationWindowValues.insert(firstIndex, newValue);
float sumer = 0.0;
for (int i = 0; i < calibrationWindowValues.size(); i++)
{
static_cast<float>(postT.translation.x) - static_cast<float>(preT.translation.x),
static_cast<float>(postT.translation.y) - static_cast<float>(preT.translation.y),
static_cast<float>(postT.translation.z) - static_cast<float>(preT.translation.z),
};
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)
{
// calculate rotation matrix
poseRotationMatrix = rotateAlign(requestedPosition, actualPosition);
......@@ -813,13 +875,17 @@ void calibratePositionReference()
float requestedPositionMag = Magnitude3(requestedPosition);
float actualPositionMag = Magnitude3(actualPosition);
poseMagnitudeScaler = (requestedPositionMag/actualPositionMag)*0.8; // aim just short of actual scalar, better to under than over shoot
poseMagnitudeScaler = (requestedPositionMag/actualPositionMag);
ROS_INFO("pose magnitude scalar %f", poseMagnitudeScaler);
calibrationWindowValues = std::vector<float> {};
poseRotationMatrixValues = std::vector<std::vector<std::vector<float>>> {{{}}};
for(int i = 0; i < calibrationWindow; i++)
{
calibrationWindowValues.push_back(poseMagnitudeScaler);
poseRotationMatrixValues.push_back(poseRotationMatrix);
}
std::vector<float> checkPosition = applyRotationMatrixScaling(requestedPosition);
relativePosition(reversePosition[0], reversePosition[1], reversePosition[2], 0.0); // go to requested position in corrected frame
ros::spinOnce();
ROS_INFO("pose magnitude scalar %f", poseMagnitudeScaler);
}
std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec)
......@@ -892,8 +958,10 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
case 7:
{
ROS_INFO("received overwatch: calibrate pos ref");
// go home
calibratePositionReference();
std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
calibratePositionReference(requestedPosition, actualPosition);
break;
}
default:
......@@ -904,6 +972,25 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
return res.result;
}
std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec)
{
// move a bit and return diff in position
ros::spinOnce(); // let callbacks update
geometry_msgs::Transform preT = ex_pos;
relativePosition(calibrationVec[0], calibrationVec[1], calibrationVec[2], 0.0);
ros::spinOnce(); // let callbacks update
geometry_msgs::Transform postT = ex_pos;
std::vector<float> deltaPosition // what we got
{
static_cast<float>(postT.translation.x) - static_cast<float>(preT.translation.x),
static_cast<float>(postT.translation.y) - static_cast<float>(preT.translation.y),
static_cast<float>(postT.translation.z) - static_cast<float>(preT.translation.z),
};
// go back
relativePosition(-calibrationVec[0], -calibrationVec[1], -calibrationVec[2], 0.0);
return deltaPosition;
}
int main(int argc, char** argv)
{
......@@ -917,6 +1004,7 @@ int main(int argc, char** argv)
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 stuff
......@@ -935,7 +1023,8 @@ int main(int argc, char** argv)
syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1);
gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>("dji_osdk_ros/gps_position", 1, &gpsPosCallback);
pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("pose_feedback", 1, &posFeedbackCallback);
pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("feedback/pose_feedback", 1, &posFeedbackCallback);
pos_tracking_subscriber = nh.subscribe<geometry_msgs::Transform>("tracking/pose_feedback", 1, &posTrackingCallback);
ros::ServiceServer overwatchService = nh.advertiseService("overwatch", overwatchFunction);
......
audio_common/sound_play/scripts/play.py
View file @ 1f96d6f8
......@@ -53,10 +53,10 @@ if __name__ == '__main__':
soundhandle = SoundClient()
rospy.sleep(1)
#rospy.sleep(1)
rospy.loginfo('Playing "%s".' % argv[1])
volume = float(argv[2]) if len(argv) == 3 else 1.0
soundhandle.playWave(argv[1], volume)
rospy.sleep(1)
#rospy.sleep(1)
audio_common/sound_play/scripts/playSound.sh 0 → 100755
View file @ 1f96d6f8
#!/bin/bash
source ~/.bashrc
echo "trying to play sound"
rosrun sound_play play.py /home/larkin/musical_drones_audio_files/'04-musicaldrones_25 or 6 to 4.wav'
audio_common/sound_play/scripts/soundplay_node.py
View file @ 1f96d6f8
......@@ -182,8 +182,8 @@ class soundtype:
position = 0
duration = 0
try:
position = self.sound.query_position(Gst.Format.TIME)[0]
duration = self.sound.query_duration(Gst.Format.TIME)[0]
position = self.sound.query_position(Gst.Format.TIME)[1]
duration = self.sound.query_duration(Gst.Format.TIME)[1]
except Exception as e:
position = 0
duration = 0
......
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