qualisys further changes

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

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

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

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

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

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

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