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(); ...@@ -123,14 +123,18 @@ bool stopMotion();
bool goHome(); 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); std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec);
bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res); 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 gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg); void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg);
void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg);
#endif // MISSION_NODE_H #endif // MISSION_NODE_H
Onboard-SDK-ROS/launch/local_test.launch
View file @ 1f96d6f8
...@@ -16,23 +16,24 @@ ...@@ -16,23 +16,24 @@
<param name="base_speed" type="double" value="0.5"/> <param name="base_speed" type="double" value="0.5"/>
<param name="min_speed" type="double" value="0.1"/> <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 --> <!-- max yaw rate in deg/s -->
<param name="max_yaw_speed" type="double" value="30.0"/> <param name="max_yaw_speed" type="double" value="30.0"/>
<param name="position_playback" type="bool" value="false"/> <param name="position_playback" type="bool" value="false"/>
<param name="drone_id" type="int" value="0"/> <param name="drone_id" type="int" value="0"/>
<param name="camera_type" type="int" value="1"/> <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="bus_driver" name="bus_driver" output="screen"/>
<!-- <node pkg="dji_osdk_ros" type="poseFeedbackConversion.py" name="poseFeedbackConversion" output="screen"> <!-- gps feedback node version -->
<rosparam command="load" param="angleOffset"> <node pkg="dji_osdk_ros" type="poseFeedbackConversion.py" name="poseFeedbackConversion" output="screen">
0.0 <rosparam command="load" param="angleOffset"> 0.0 </rosparam>
</rosparam> <rosparam command="load" param="anchorPoint"> [37.196986, -80.578257, 100.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>
<!-- 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"> <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.196986, -80.578257] </rosparam>
......
Onboard-SDK-ROS/scripts/poseFeedbackConversion.py
View file @ 1f96d6f8
...@@ -15,10 +15,11 @@ from tools import lat_lon2meters, meters2lat_lon ...@@ -15,10 +15,11 @@ from tools import lat_lon2meters, meters2lat_lon
import math import math
homeSetFlag = False homeSetFlag = False
# homePoint = PointStamped()
homePoint = NavSatFix() homePoint = NavSatFix()
transform = Transform() transform = Transform()
quat = Quaternion() quat = Quaternion()
translation = Vector3()
posePublisher = rospy.Publisher("pose_feedback", Transform, queue_size = 1)
angleOffset = 0.0 angleOffset = 0.0
def point_rotation(origin, pt, ang): def point_rotation(origin, pt, ang):
...@@ -35,51 +36,34 @@ def point_rotation(origin, pt, ang): ...@@ -35,51 +36,34 @@ def point_rotation(origin, pt, ang):
def rotCallback(data): def rotCallback(data):
global quat global quat
quat = data.orientation; quat = data.quaternion;
# # FINE ill DO it MYself all ALONE
def posCallback(data): def posCallback(data):
global homeSetFlag global quat
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...")
# convert and compare against home point # convert and compare against home point
translation = Vector3() homePoint = rospy.get_param("~anchorPoint")
rotation = Quaternion() homePointMeters = lat_lon2meters([homePoint[0], homePoint[1]])
# rotate the xy plane by angle
homePointMeters = lat_lon2meters([homePoint.latitude, homePoint.longitude])
currentPointMeters = lat_lon2meters([data.latitude, data.longitude]) currentPointMeters = lat_lon2meters([data.latitude, data.longitude])
currentOffset = [currentPointMeters[0] - homePointMeters[0], currentPointMeters[1] - homePointMeters[1]] currentOffset = [currentPointMeters[0] - homePointMeters[0], currentPointMeters[1] - homePointMeters[1]]
angleOffset = rospy.get_param("~angleOffset") angleOffset = rospy.get_param("~angleOffset")
currentRotatedOffset = point_rotation([0,0], currentOffset, angleOffset) currentRotatedOffset = point_rotation([0,0], currentOffset, angleOffset)
# performing dark magics # performing dark magics
translation = Vector3()
translation.x = currentRotatedOffset[1] translation.x = currentRotatedOffset[1]
translation.y = currentRotatedOffset[0] translation.y = currentRotatedOffset[0]
translation.z = data.altitude - homePoint.altitude translation.z = data.altitude - homePoint[2]
transform.translation = translation transform.translation = translation
transform.rotation = quat transform.rotation = quat
posePublisher.publish(transform)
def main(): def main():
global transform
rospy.init_node("poseFeedbackConversion") rospy.init_node("poseFeedbackConversion")
rospy.Subscriber("dji_osdk_ros/gps_position", NavSatFix, posCallback, queue_size = 1) rospy.Subscriber("dji_osdk_ros/gps_position", NavSatFix, posCallback, queue_size = 1)
rospy.Subscriber("dji_osdk_ros/imu", Imu, rotCallback, queue_size = 1) rospy.Subscriber("dji_osdk_ros/attitude", QuaternionStamped, rotCallback, queue_size = 1)
posePublisher = rospy.Publisher("pose_feedback", Transform, queue_size = 1) rospy.spin()
while not rospy.is_shutdown(): # get anchor point from params
posePublisher.publish(transform)
if __name__ == '__main__': if __name__ == '__main__':
main() main()
Onboard-SDK-ROS/scripts/qualisysFeedback.py
View file @ 1f96d6f8
...@@ -10,21 +10,21 @@ def callback(data): ...@@ -10,21 +10,21 @@ def callback(data):
# 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
# transform.translation = data.pose.position #transform.translation = data.pose.position
transform.translation.x = data.pose.position.z transform.translation.x = data.pose.position.x
transform.translation.y = data.pose.position.z
transform.translation.z = data.pose.position.y transform.translation.z = data.pose.position.y
transform.translation.y = data.pose.position.x
def main(): def main():
global transform global transform
rospy.init_node("qualisysFeedback") rospy.init_node("qualisysFeedback", anonymous = True)
name = rospy.get_param("~trackName") name = rospy.get_param("~trackName")
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("/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1) rospy.Subscriber("/" + namespace + "/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
pub = rospy.Publisher("pose_feedback", Transform, queue_size = 1) pub = rospy.Publisher(namespace + "/pose_feedback", Transform, queue_size = 1)
while not rospy.is_shutdown(): while not rospy.is_shutdown():
pub.publish(transform) pub.publish(transform)
if __name__ == '__main__': if __name__ == '__main__':
main() main()
Onboard-SDK-ROS/scripts/waypointGeneratorRemote.py
View file @ 1f96d6f8
...@@ -20,9 +20,9 @@ def generateWaypoints(): ...@@ -20,9 +20,9 @@ def generateWaypoints():
# musical drones trajectory is convient test case, but general path is possible # musical drones trajectory is convient test case, but general path is possible
# ring sizes # ring sizes
big = 10 big = 5
med = 5 med = 3
sml = 3 sml = 1
apMeters = lat_lon2meters(anchorPoint) apMeters = lat_lon2meters(anchorPoint)
......
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
View file @ 1f96d6f8
/** @file mission_node.cpp /** @file mission_node.cpp
* @version 4.0 * @version 4.0
* @date June 2020 * @date June 2020
...@@ -54,9 +55,22 @@ ros::Publisher syncPub; // thing to publish to sync topic "positionSync" ...@@ -54,9 +55,22 @@ 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)
float externalYaw; // yaw as measured by pos ref geometry_msgs::Transform track_pos; // position from external source (qualisys or some such)
float externalPitch; // pitch as measured by pos ref float feedbackYaw; // yaw as measured by pos ref
float externalRoll; // roll 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 gps_home_flag = false;
bool pos_home_flag = false; bool pos_home_flag = false;
bool positionMissionFlag = false; bool positionMissionFlag = false;
...@@ -64,6 +78,7 @@ bool waypointMissionFlag = false; ...@@ -64,6 +78,7 @@ bool waypointMissionFlag = false;
bool positionPlaybackParam = false; bool positionPlaybackParam = false;
ros::Subscriber gps_pos_subscriber; ros::Subscriber gps_pos_subscriber;
ros::Subscriber pos_feedback_subscriber; ros::Subscriber pos_feedback_subscriber;
ros::Subscriber pos_tracking_subscriber;
int droneId; // stupid but will work for now int droneId; // stupid but will work for now
int waypointMissionUpdateCounter = 0; // counts iterations before we check waypoints again int waypointMissionUpdateCounter = 0; // counts iterations before we check waypoints again
...@@ -82,11 +97,6 @@ double speedScalarParam; ...@@ -82,11 +97,6 @@ double speedScalarParam;
double minYawSpeedParam; double minYawSpeedParam;
double maxYawSpeedParam; 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 // 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)
...@@ -322,17 +332,19 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg) ...@@ -322,17 +332,19 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
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
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)); 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));
externalPitch = asin(2.0*(ex_pos.rotation.x*ex_pos.rotation.z - ex_pos.rotation.w*ex_pos.rotation.y)); feedbackPitch = 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)); 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); void posTrackingCallback(const geometry_msgs::Transform::ConstPtr& msg)
if (!positionMissionFlag) {
{ track_pos = *msg;
return; // peace out if not running mission
}
// 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) bool setupWaypointMission(int responseTimeout)
...@@ -402,80 +414,87 @@ bool runPositionMission() ...@@ -402,80 +414,87 @@ bool runPositionMission()
if (positionPlaybackParam) if (positionPlaybackParam)
{ {
// replaying a capture from reference WIP // replaying a capture from reference WIP
// ros::Rate updateRate(20); // update period for topics/commands // executing a set of positions in reference
// while (positionMissionFlag && ros::ok()) ros::Rate updateRate(20); // update period for topics/commands
// { positionMissionFlag = true;
// // print distance to first position
// std::vector<float> currentPos while (positionMissionFlag && ros::ok())
// { {
// static_cast<float>(ex_pos.translation.x), std::vector<float> currentPos
// static_cast<float>(ex_pos.translation.y), {
// static_cast<float>(ex_pos.translation.z) static_cast<float>(ex_pos.translation.x),
// }; static_cast<float>(ex_pos.translation.y),
// std::vector<float> tempPos; static_cast<float>(ex_pos.translation.z)
// ros::param::get("/robot_list/robot_" + to_string(droneId) + "/positions/position_" + to_string(currentPositionIndex), tempPos); };
// // pull out x,y,z
// std::vector<float> targetPos std::vector<float> targetPos
// { {
// tempPos[1], // 0th index is the index of position, facepalm static_cast<float>(track_pos.translation.x),
// tempPos[2], static_cast<float>(track_pos.translation.y),
// tempPos[3] static_cast<float>(track_pos.translation.z)
// }; };
//
// if (!rotationMatrixCalibratedFlag) // could also do this before loop? if (!rotationMatrixCalibratedFlag) // could also do this before loop?
// { {
// // run rotation matrix calc // run rotation matrix calc
// calibratePositionReference(); std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
// rotationMatrixCalibratedFlag = true; 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(trackingYaw); // might need to use different axis here, bc rotations
// float targetYaw = applyRotationAngle(normalizeAngle((M_PI/180.0)*tempPos[4])); float currentYaw = feedbackYaw;
// float currentYaw = externalYaw; ROS_INFO("current postion %f, %f, %f", currentPos[0], currentPos[1], currentPos[2]);
// ROS_INFO("target yaw: %f", targetYaw); ROS_INFO("target position %f, %f, %f", targetPos[0], targetPos[1], targetPos[2]);
// ROS_INFO("current yaw: %f", currentYaw); ROS_INFO("target yaw: %f", targetYaw);
// ROS_INFO("current yaw: %f", currentYaw);
// float yawCommand = normalizeAngle(currentYaw - targetYaw);
// // check dir we should go float yawCommand = normalizeAngle(currentYaw - targetYaw);
// ROS_INFO("yaw command: %f", yawCommand); // 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); // convert both target and current position, and move between
// std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos); std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
// // map yaw command (in deg) to rotation offset (in rads) std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// // float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis // map yaw command (in deg) to rotation offset (in rads)
//
// std::vector<float> moveCommand std::vector<float> moveCommand
// { {
// targetPosMapped[0] - currentPosMapped[0], targetPosMapped[0] - currentPosMapped[0],
// targetPosMapped[1] - currentPosMapped[1], targetPosMapped[1] - currentPosMapped[1],
// targetPosMapped[2] - currentPosMapped[2] targetPosMapped[2] - currentPosMapped[2]
// }; };
//
// // go to position // go to position
// relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand); ROS_INFO("calling move command w %f, %f, %f", moveCommand[0], moveCommand[1], moveCommand[2]);
// relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
// ros::spinOnce();
// // let topics update
// // check if we're close to position ros::spinOnce();
// float checkDist = eucDistance(currentPos, targetPos);
// if (checkDist <= 0.2 && abs(currentYaw - targetYaw) <= 0.1) // close ish // check if position call was correct in terms of distance TODO also do this process for the rotation matrix
// { // ---------------------------------
// currentPositionIndex += 1; // go to next position std::vector<float> calCheckPosition
// } {
// static_cast<float>(ex_pos.translation.x),
// if (currentPositionIndex >= numPositions) static_cast<float>(ex_pos.translation.y),
// { static_cast<float>(ex_pos.translation.z)
// // leave };
// positionMissionFlag = false; // what the distance actually was in ref coords
// } float refDist = eucDistance(currentPos, calCheckPosition);
// // ROS_INFO("distance to first position: %f", firstPosDist); // what the distance was in command coords
// float cmdDist = Magnitude3(moveCommand);
// if (cmdDist > stdMoveDist)
// updateRate.sleep(); {
// } cmdDist = stdMoveDist;
}
// if movecommand, mapped to coords, does not match up with difference in position, scream
updateMagnitudeScaler(refDist/cmdDist);
// ---------------------------------
updateRate.sleep();
}
} }
else else
{ {
...@@ -488,7 +507,6 @@ bool runPositionMission() ...@@ -488,7 +507,6 @@ bool runPositionMission()
while (positionMissionFlag && ros::ok()) while (positionMissionFlag && ros::ok())
{ {
// print distance to first position
std::vector<float> currentPos std::vector<float> currentPos
{ {
static_cast<float>(ex_pos.translation.x), static_cast<float>(ex_pos.translation.x),
...@@ -508,15 +526,17 @@ bool runPositionMission() ...@@ -508,15 +526,17 @@ bool runPositionMission()
if (!rotationMatrixCalibratedFlag) // could also do this before loop? if (!rotationMatrixCalibratedFlag) // could also do this before loop?
{ {
// run rotation matrix calc // 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; rotationMatrixCalibratedFlag = true;
} }
// may need to rotate this yaw to match coordinate system // 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 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("target yaw: %f", targetYaw);
ROS_INFO("current yaw: %f", currentYaw); ROS_INFO("current yaw: %f", currentYaw);
...@@ -525,8 +545,10 @@ bool runPositionMission() ...@@ -525,8 +545,10 @@ bool runPositionMission()
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
// ROS_INFO("applying rotation matrix scaling...");
std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos); std::vector<float> targetPosMapped = applyRotationMatrixScaling(targetPos);
std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos); std::vector<float> currentPosMapped = applyRotationMatrixScaling(currentPos);
// ROS_INFO("successfully applied rotation matrix");
// map yaw command (in deg) to rotation offset (in rads) // 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 // float targetYawMapped = applyRotationAngle(targetYaw); // this assumes much, like that we're rotating about the pos z axis
...@@ -538,13 +560,36 @@ bool runPositionMission() ...@@ -538,13 +560,36 @@ bool runPositionMission()
}; };
// go to position // 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); relativePosition(moveCommand[0], moveCommand[1], moveCommand[2], yawCommand);
// let topics update
ros::spinOnce(); 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 // check if we're close to position
float checkDist = eucDistance(currentPos, targetPos); 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 currentPositionIndex += 1; // go to next position
} }
...@@ -707,7 +752,9 @@ float applyRotationAngle(float localYaw) ...@@ -707,7 +752,9 @@ float applyRotationAngle(float localYaw)
if (!rotationMatrixCalibratedFlag) // could also do this before loop? if (!rotationMatrixCalibratedFlag) // could also do this before loop?
{ {
// run rotation matrix calc // 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; rotationMatrixCalibratedFlag = true;
} }
return localYaw + poseRotationRadians; return localYaw + poseRotationRadians;
...@@ -718,9 +765,19 @@ bool relativePosition(float x, float y, float z, float yaw) ...@@ -718,9 +765,19 @@ bool relativePosition(float x, float y, float z, float yaw)
// convert relative position to velocity command // convert relative position to velocity command
std::vector<float> posVec {x, y, z}; std::vector<float> posVec {x, y, z};
float totalDist = Magnitude3(posVec); 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 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);
...@@ -729,8 +786,6 @@ bool relativePosition(float x, float y, float z, float yaw) ...@@ -729,8 +786,6 @@ bool relativePosition(float x, float y, float z, float yaw)
dji_osdk_ros::FlightTaskControl flightTaskControl; dji_osdk_ros::FlightTaskControl flightTaskControl;
dji_osdk_ros::JoystickParams joystickParams; dji_osdk_ros::JoystickParams joystickParams;
// float sentYaw = ((abs(yaw)/yaw)*yawSpeed)/(totalTimeMs/1000.0);
joystickParams.x = velVec[0]; joystickParams.x = velVec[0];
joystickParams.y = velVec[1]; joystickParams.y = velVec[1];
joystickParams.z = velVec[2]; joystickParams.z = velVec[2];
...@@ -746,14 +801,16 @@ bool relativePosition(float x, float y, float z, float yaw) ...@@ -746,14 +801,16 @@ bool relativePosition(float x, float y, float z, float yaw)
totalTimeMs = (abs(yaw)/tempYawRate)*1000.0; totalTimeMs = (abs(yaw)/tempYawRate)*1000.0;
} }
} }
ROS_INFO("yaw rate: %f over %f sec", tempYawRate, (totalTimeMs/1000.0));
ROS_INFO("---------------\n");
joystickParams.yaw = tempYawRate; joystickParams.yaw = tempYawRate;
} }
else else
{ {
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("---------------\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;
flightTaskControl.request.joystickCommand = joystickParams; flightTaskControl.request.joystickCommand = joystickParams;
flightTaskControl.request.velocityControlTimeMs = totalTimeMs; flightTaskControl.request.velocityControlTimeMs = totalTimeMs;
...@@ -788,23 +845,28 @@ bool goHome() ...@@ -788,23 +845,28 @@ bool goHome()
return true; 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 void updateMagnitudeScaler(float newValue)
std::vector<float> reversePosition {-0.5, 0.0, 0.0}; // what we asked for {
std::vector<float> actualPosition // what we got // 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), sumer += calibrationWindowValues[i];
static_cast<float>(postT.translation.y) - static_cast<float>(preT.translation.y), }
static_cast<float>(postT.translation.z) - static_cast<float>(preT.translation.z), 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 // calculate rotation matrix
poseRotationMatrix = rotateAlign(requestedPosition, actualPosition); poseRotationMatrix = rotateAlign(requestedPosition, actualPosition);
...@@ -813,13 +875,17 @@ void calibratePositionReference() ...@@ -813,13 +875,17 @@ void calibratePositionReference()
float requestedPositionMag = Magnitude3(requestedPosition); float requestedPositionMag = Magnitude3(requestedPosition);
float actualPositionMag = Magnitude3(actualPosition); 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); ROS_INFO("pose magnitude scalar %f", poseMagnitudeScaler);
relativePosition(reversePosition[0], reversePosition[1], reversePosition[2], 0.0); // go to requested position in corrected frame
ros::spinOnce();
} }
std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec) std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec)
...@@ -892,8 +958,10 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over ...@@ -892,8 +958,10 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
case 7: case 7:
{ {
ROS_INFO("received overwatch: calibrate pos ref"); 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; break;
} }
default: default:
...@@ -904,6 +972,25 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over ...@@ -904,6 +972,25 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
return res.result; 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) int main(int argc, char** argv)
{ {
...@@ -917,6 +1004,7 @@ 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("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_INFO("base speed set to %f", baseSpeedParam); // ROS_INFO("base speed set to %f", baseSpeedParam);
// ROS stuff // ROS stuff
...@@ -935,7 +1023,8 @@ int main(int argc, char** argv) ...@@ -935,7 +1023,8 @@ int main(int argc, char** argv)
syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1); syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1);
gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>("dji_osdk_ros/gps_position", 1, &gpsPosCallback); 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); ros::ServiceServer overwatchService = nh.advertiseService("overwatch", overwatchFunction);
......
audio_common/sound_play/scripts/play.py
View file @ 1f96d6f8
...@@ -53,10 +53,10 @@ if __name__ == '__main__': ...@@ -53,10 +53,10 @@ if __name__ == '__main__':
soundhandle = SoundClient() soundhandle = SoundClient()
rospy.sleep(1) #rospy.sleep(1)
rospy.loginfo('Playing "%s".' % argv[1]) rospy.loginfo('Playing "%s".' % argv[1])
volume = float(argv[2]) if len(argv) == 3 else 1.0 volume = float(argv[2]) if len(argv) == 3 else 1.0
soundhandle.playWave(argv[1], volume) 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: ...@@ -182,8 +182,8 @@ class soundtype:
position = 0 position = 0
duration = 0 duration = 0
try: try:
position = self.sound.query_position(Gst.Format.TIME)[0] position = self.sound.query_position(Gst.Format.TIME)[1]
duration = self.sound.query_duration(Gst.Format.TIME)[0] duration = self.sound.query_duration(Gst.Format.TIME)[1]
except Exception as e: except Exception as e:
position = 0 position = 0
duration = 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