From 4d7bba57ea2ef40b70c4bb7967a127e345038f28 Mon Sep 17 00:00:00 2001
From: cesar <cesar.rodriguez@spirirobotics.com>
Date: Fri, 16 Sep 2022 12:21:27 -0300
Subject: [PATCH] removing mocap files - Not relavent to spiri

---
 .gitignore                     |  5 ++--
 CMakeLists.txt                 | 12 +++-------
 launch/mocap_sim.launch        |  4 ++++
 launch/takeoff_noCtrl.launch   | 44 ----------------------------------
 src/MoCap_Localization_fake.py |  4 ++++
 src/klausen_control.py         |  5 ++--
 src/mocap_path_follow.cpp      |  8 ++++++-
 7 files changed, 24 insertions(+), 58 deletions(-)
 delete mode 100644 launch/takeoff_noCtrl.launch

diff --git a/.gitignore b/.gitignore
index c4132b6..6c0440d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,7 +1,7 @@
 config/mocap*
 launch/cortex_bridge.launch
-launch/debug.launch
-launch/klausen_dampen.launch
+launch/headless_spiri_mocap.launch
+launch/headless_spiri_with_tether_mocap.launch
 launch/mocap_*
 src/development_*
 src/killswitch_client.py
@@ -15,4 +15,5 @@ msg/Marker.msg
 msg/Markers.msg
 *.rviz
 setup.txt
+px4_setup/protobuf_install.txt
 
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2efbd8b..077d719 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -84,17 +84,11 @@ target_link_libraries(pathFollow_node ${catkin_LIBRARIES})
 add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
 #add_dependencies(pathFollow_node oscillation_ctrl_generate_messages_cpp)
 
-#add_executable(mocap_offb_node src/mocap_offb_node.cpp)
+add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp)
 
-#target_link_libraries(mocap_offb_node ${catkin_LIBRARIES})
+target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES})
 
-#add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
-
-#add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp)
-
-#target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES})
-
-#add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
diff --git a/launch/mocap_sim.launch b/launch/mocap_sim.launch
index 6cd2bdd..6a06a84 100644
--- a/launch/mocap_sim.launch
+++ b/launch/mocap_sim.launch
@@ -56,5 +56,9 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 	<!-- PX4 LAUNCH -->
 	<include file="$(find oscillation_ctrl)/launch/$(arg model)_mocap.launch">
 	</include>
+	
+	<!-- Tf LAUNCH -->
+	<node pkg="tf" type="static_transform_publisher" name="map_to_pload_imu" args="0 0 0 0 0 0 map pload_imu 10" />
+	<node pkg="tf" type="static_transform_publisher" name="map_to_pload" args="0 0 0 0 0 0 map payload 10" />
 </launch>
 
diff --git a/launch/takeoff_noCtrl.launch b/launch/takeoff_noCtrl.launch
deleted file mode 100644
index f899656..0000000
--- a/launch/takeoff_noCtrl.launch
+++ /dev/null
@@ -1,44 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <arg name="model" default="headless_spiri_with_tether"/>
-  <arg name='test' default="none"/>
-  
-	<group ns="sim">
-  	<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
-	</group>
-  <group ns="status">
-		<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
-  	<param name="test_type" value="$(arg test)"/>
-  </group>
-
-	<node
-		pkg="oscillation_ctrl"
-		type="LinkState.py"
-		name="LinkStates"
-		launch-prefix="xterm -e"
-	/>
-	<node
-		pkg="oscillation_ctrl"
-		type="offb_node"
-		name="offb_node"
-		launch-prefix="xterm -e"
-	/>
-
-
-	<node
-		pkg="oscillation_ctrl"
-		type="wpoint_tracker.py"
-		name="waypoints_server"
-	/>
-
-	<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 px4)/launch/$(arg model).launch"/>
-</launch>
diff --git a/src/MoCap_Localization_fake.py b/src/MoCap_Localization_fake.py
index 9a3134d..5478d40 100755
--- a/src/MoCap_Localization_fake.py
+++ b/src/MoCap_Localization_fake.py
@@ -200,7 +200,11 @@ class Main:
 		# fill out necesssary fields
 		self.drone_pose.header.frame_id = "/map"
 		self.drone_pose.header.stamp = rospy.Time.now()
+		# load angles
 		self.load_angles.header.stamp = rospy.Time.now()
+		# twobody status
+		self.twobody_status.header.frame_id = "/map"
+		self.twobody_status.header.stamp = rospy.Time.now()
 		# publish
 		self.visionPose_pub.publish(self.drone_pose)  # publish pose to mavros
 		self.loadAng_pub.publish(self.load_angles)    # publish load angles to controller
diff --git a/src/klausen_control.py b/src/klausen_control.py
index d8307ec..a2cf106 100755
--- a/src/klausen_control.py
+++ b/src/klausen_control.py
@@ -15,7 +15,7 @@ import rosservice
 
 from tf.transformations   import *
 from scipy.integrate  	  import odeint 
-from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus
+from oscillation_ctrl.msg import RefSignal, LoadAngles
 from oscillation_ctrl.srv import WaypointTrack
 from geometry_msgs.msg 	  import Pose, Point, TwistStamped, PoseStamped
 from sensor_msgs.msg 	  import Imu
@@ -185,7 +185,8 @@ class Main:
 		if rospy.has_param('status/hover_throttle'):
 			hover_throttle = rospy.get_param('status/hover_throttle')
 		else:
-			hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass
+			hover_throttle = (self.tot_m*9.81 + 11.2)/34.84 # linear scaling depending on dependent on mass
+			rospy.set_param('status/hover_throttle',hover_throttle)
 		kf = hover_throttle/(self.tot_m*9.81)
 		return kf
 
diff --git a/src/mocap_path_follow.cpp b/src/mocap_path_follow.cpp
index 7d1035c..0e33402 100644
--- a/src/mocap_path_follow.cpp
+++ b/src/mocap_path_follow.cpp
@@ -182,8 +182,14 @@ int main(int argc, char **argv)
 					takeoff = true;
 				}
 			}
+			if (waypoint_cl.call(wpoint_srv)){
+			// check if waypoints have changed
+			buff_pose.pose.position.x = wpoint_srv.response.xd.x;
+			buff_pose.pose.position.y = wpoint_srv.response.xd.y;
+			buff_pose.pose.position.z = wpoint_srv.response.xd.z;
+			alt_des = wpoint_srv.response.xd.z;
+			}
 			// Check if we want to use oscillation controller
-			//if (ros::param::get("/use_ctrl", oscillation_damp) == true){
 			if (ros::param::has("/status/use_ctrl")){
 				ros::param::get("/status/use_ctrl", oscillation_damp);
 				if(oscillation_damp){