From 76ce88cc4ee796d6578c2a6d2d48ebca9826b0c3 Mon Sep 17 00:00:00 2001
From: Matas Keras <matas.keras@spirirobotics.com>
Date: Wed, 13 Jul 2022 10:11:01 -0400
Subject: [PATCH] Added src and launch files

---
 launch/oscillation_damp_circle.launch | 86 +++++++++++++++++++++++++++
 src/circle_waypoints.py               | 27 +++++++++
 2 files changed, 113 insertions(+)
 create mode 100644 launch/oscillation_damp_circle.launch
 create mode 100755 src/circle_waypoints.py

diff --git a/launch/oscillation_damp_circle.launch b/launch/oscillation_damp_circle.launch
new file mode 100644
index 0000000..41c260c
--- /dev/null
+++ b/launch/oscillation_damp_circle.launch
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+<!--
+Launch file to use klausen oscillaton damping ctrl in Gazebo
+/-->
+<launch>
+  <arg name="model" default="headless_spiri_with_tether"/>
+  <arg name="test" default="none"/>
+  <arg name="turtle" default="none"/>
+  <group ns="sim">
+  	<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
+  </group>
+
+  <group ns="status">
+  	<param name="test_type" value="$(arg test)"/>
+  </group>
+
+
+
+	<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
+	<node
+		pkg="oscillation_ctrl"
+		type="LinkState.py"
+		name="linkStates_node"
+		launch-prefix="xterm -e"
+	/>
+	<!-- DETERMINES DESIRED POSITION -->
+	<node
+		pkg="oscillation_ctrl"
+		type="wpoint_tracker.py"
+		name="waypoints_server"
+	/>
+	<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
+	<node
+		pkg="oscillation_ctrl"
+		type="ref_signalGen.py"
+		name="refSignal_node"
+	/>
+	<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
+	<node
+		pkg="oscillation_ctrl"
+		type="klausen_control.py"
+		name="klausenCtrl_node"
+		output="screen"
+	/>
+	<!-- PUBLISHES DESIRED COMMANDS -->
+	<node
+		pkg="oscillation_ctrl"
+		type="pathFollow_node"
+		name="pathFollow_node"
+		launch-prefix="xterm -e"
+	/>
+
+
+	<!-- PUBLISHES WAYPOINTS FOR CIRCLE -->
+	<node
+		pkg="oscillation_ctrl"
+		type="circle_waypoints.py"
+		name="circle_waypoints"
+		/>
+	<group if="$(eval turtle != 'none')">
+		
+		<!-- TAKES DRONE POSITION, AND PASSES IT TO TURTLE-->
+		<node 
+			pkg="oscillation_ctrl"
+			type="turtle_test.py"
+			name="turtle_test"
+		/>
+		<!-- TURTLE NODE -->
+		<node
+			pkg="turtlesim"
+			type="turtlesim_node"
+			name="turtlesim_node"
+		/>
+	</group>
+	<!-- RUNS DIFFERENT TESTS IF DESIRED -->
+	<group if="$(eval test != 'none')">
+		<node
+			pkg="oscillation_ctrl"
+			type="perform_test.py"
+			name="test_node"
+			launch-prefix="xterm -e"
+		/>
+	</group>
+	<!-- PX4 LAUNCH -->
+	<include file="$(find oscillation_ctrl)/launch/$(arg model).launch"/>
+</launch>
diff --git a/src/circle_waypoints.py b/src/circle_waypoints.py
new file mode 100755
index 0000000..30b6327
--- /dev/null
+++ b/src/circle_waypoints.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+# license removed for brevity
+import rospy
+from std_msgs.msg import String
+from geometry_msgs.msg import Point
+import numpy as np
+def circle_points():
+    
+    pub = rospy.Publisher('reference/waypoints',Point, queue_size=10)
+    rospy.init_node('circle_points', anonymous=True)
+    rospy.loginfo('init_node')
+    rate=rospy.Rate(10)
+    rospy.sleep(48.)
+    now = rospy.get_rostime().secs
+    while not rospy.is_shutdown():
+        x_pos = np.cos((rospy.get_rostime().secs-now)*np.pi/30)-1
+        y_pos = np.sin((rospy.get_rostime().secs-now)*np.pi/30)
+        z_pos = 5
+        to_publish = Point(x=x_pos,y=y_pos,z=z_pos)
+        pub.publish(to_publish)
+        rate.sleep
+
+if __name__ == '__main__':
+    try:
+        circle_points()
+    except rospy.ROSInterruptException:
+        pass