cube testing updates

Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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)
 

audio_common/sound_play/scripts/play.py

@@ -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

+#!/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

@@ -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