added service node to keep track of desired waypoints

.gitignore

@@ -1,2 +1,3 @@
-
+launch/debug.launch
 src/MoCap_*.py
+*.rviz

CMakeLists.txt

@@ -10,6 +10,7 @@ project(oscillation_ctrl)
 find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   mavros
+  message_generation
   message_runtime
   roscpp
 )
@@ -56,11 +57,10 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
+ add_service_files(
-#   FILES
+   FILES
-#   Service1.srv
+   WaypointTrack.srv
-#   Service2.srv
+ )
-# )
 
 ## Generate actions in the 'action' folder
 # add_action_files(

config/Ctrl_param.yaml

@@ -1,3 +1,5 @@
 # Ros param when using Klausen Ctrl
 
-wait: 52
+wait: 52
+waypoints: {x:  0.0, y:  0.0, z:  3.0}
+

config/noCtrl_param.yaml

@@ -1,3 +1,4 @@
 # Ros param when not using Klausen Ctrl
 
 wait: 55 
+waypoints: {x:  0.0, y:  0.0, z:  3.0}

frames.gv

@@ -0,0 +1,9 @@
+digraph G {
+"local_origin" -> "local_origin_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
+"fcu" -> "fcu_frd"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
+"fcu_frd" -> "fcu"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 124.428"[ shape=plaintext ] ;
+ }->"local_origin";
+}

launch/klausen_dampen.launch

@@ -10,6 +10,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
   <arg name="respawn_mavros" default="false" />
   <arg name="gazebo_gui" default="false" />
   <arg name="test_type" default="step.py" />
+  <arg name="model" default="spiri_with_tether"/>
 
   <group ns="sim">
   	<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
@@ -59,5 +60,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
           <param name="Kv_z" value="6.0" />
   	</node>
 	<!-- PX4 LAUNCH -->
-	<include file="$(find px4)/launch/spiri_with_tether.launch"/>
+	<include file="$(find px4)/launch/$(arg model).launch"/>
-</launch>
+</launch>

launch/mocap_sim.launch

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<!--
+Launch file to use klausen oscillaton damping ctrl in Gazebo
+/-->
+<launch>
+  <arg name="mav_name" default="spiri_mocap"/>
+  <arg name="command_input" default="1" />
+
+	<node
+		pkg="oscillation_ctrl"
+		type="MoCap_Localization_fake.py"
+		name="fakeMocap_node"
+		launch-prefix="xterm -e"
+	/>
+	<!--node
+		pkg="oscillation_ctrl"
+		type="offb_node"
+		name="offb_node"
+		launch-prefix="xterm -e"
+	/-->
+	
+  	<!--node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
+  	  <param name="mav_name" type="string" value="$(arg mav_name)" />
+          <!-remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/->
+          <param name="ctrl_mode" value="$(arg command_input)" />
+          <param name="max_acc" value="8.0" />
+          <param name="Kp_x" value="8.0" />
+          <param name="Kp_y" value="8.0" />
+          <param name="Kp_z" value="10.0" />
+          <param name="Kv_x" value="3.0" />
+          <param name="Kv_y" value="3.0" />
+          <param name="Kv_z" value="6.0" />
+  	</node-->
+	<!-- PX4 LAUNCH -->
+	<include file="$(find px4)/launch/headless_spiri_mocap.launch"/>
+</launch>

launch/takeoff_noCtrl.launch

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="test_type" default="step.py" />
-
+  <arg name="model" default="spiri"/>
   <group ns="sim">
   	<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
   </group>
@@ -24,4 +24,7 @@
 		name="test_node"
 		launch-prefix="xterm -e"
 	/>
+
+	<!-- PX4 LAUNCH -->
+	<include file="$(find px4)/launch/$(arg model).launch"/>
 </launch>

src/LinkState.py

@@ -11,6 +11,7 @@ from tf.transformations import *
 from offboard_ex.msg import tethered_status
 from geometry_msgs.msg import Pose
 from gazebo_msgs.srv import GetLinkState
+from oscillation_ctrl.srv import WaypointTrack
 from std_msgs.msg import Bool
 
 class Main:
@@ -55,11 +56,7 @@ class Main:
 		# variables for message gen
 		self.status 					= tethered_status()
 		self.status.drone_id	= 'spiri_with_tether::spiri::base'
-		self.status.drone_pos	= Pose()
 		self.status.pload_id	= 'spiri_with_tether::mass::payload'
-		self.status.pload_pos	= Pose()
-		self.status.phi				= 0.0
-		self.status.theta 		= 0.0
 
 		# service(s)
 		self.service1 = '/gazebo/get_link_state'

src/MoCap_Localization_noTether.py

@@ -41,8 +41,19 @@ class Main:
 		### -*-*-*- END -*-*-*- ###
 
 		# initialize variables
-		self.buff_pose  = PoseStamped()
+		#self.buff_pose1  = PoseStamped()
 		self.drone_pose = PoseStamped()
+		self.buff_pose  = PoseStamped()
+
+		# Debugging
+#		self.buff_pose.pose.position.y = 0
+#		self.buff_pose.pose.position.x = 0
+#		self.buff_pose.pose.position.z = 0
+#		self.buff_pose.pose.orientation.y = 0
+#		self.buff_pose.pose.orientation.x = 0
+#		self.buff_pose.pose.orientation.z = 0
+#		self.buff_pose.pose.orientation.w = 1
+#		self.buff_pose.header.frame_id = 'map'
 
 		# Max dot values to prevent 'blowup'
 		self.phidot_max   = 3.0
@@ -110,15 +121,18 @@ class Main:
 		'''
 		Transforms mocap reading to proper coordinate frame
 		'''
-		self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
+
-		self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
+		self.drone_pose = self.buff_pose
-		self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
+		self.drone_pose.header.frame_id = "map"
+#		self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
+#		self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
+#		self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
 
 #		Keep the w same and change x, y, and z as above.
-		self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y
+#		self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y
-		self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x
+#		self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x
-		self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z
+#		self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z
-		self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w
+#		self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w
   
 #  def path_follow(self,path_timer):
 #		now = rospy.get_time()
@@ -138,6 +152,12 @@ class Main:
 	def publisher(self,pub_timer):
 		self.FRD_Transform()
 		self.pose_pub.publish(self.drone_pose)
+		print "\n"
+		rospy.loginfo("")
+		print "drone pos.x:    " + str(round(self.drone_pose.pose.position.x,2))
+		print "drone pos.y:    " + str(round(self.drone_pose.pose.position.y,2))
+		print "drone pos.z:    " + str(round(self.drone_pose.pose.position.z,2))
+
 
 if __name__=="__main__":
 

src/klausen_control.py

@@ -75,7 +75,7 @@ class Main:
 		self.K1					= np.identity(3)
 		self.K2					= np.identity(5)
 
-		# Values which require updates *_p = prior
+		# Values which require updates. *_p = prior
 		self.z1_p			=	np.zeros([3,1])
 		self.a45_0		= np.zeros(2)
 		self.alpha		= np.zeros([5,1])
@@ -308,9 +308,9 @@ class Main:
 
 		# Attitude control
 		self.quaternion.header.stamp = rospy.Time.now()
-		self.quaternion.pose.position.x 	 = 0
+#		self.quaternion.pose.position.x 	 = 0
-		self.quaternion.pose.position.y 	 = 0
+#		self.quaternion.pose.position.y 	 = 0
-		self.quaternion.pose.position.z 	 = 5.0
+#		self.quaternion.pose.position.z 	 = 5.0
 		self.quaternion.pose.orientation.x = q[0] 
 		self.quaternion.pose.orientation.y = q[1]
 		self.quaternion.pose.orientation.z = q[2]

src/offb_node.cpp

@@ -115,7 +115,7 @@ int main(int argc, char **argv)
     // Populate pose msg
     pose.pose.position.x = 0;
     pose.pose.position.y = 0;
-    pose.pose.position.z = 5.0;
+    pose.pose.position.z = 1.0;
     pose.pose.orientation.x = q_init.x;
     pose.pose.orientation.y = q_init.y;
     pose.pose.orientation.z = q_init.z;

src/path_follow.cpp

@@ -144,10 +144,14 @@ int main(int argc, char **argv)
     attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO 
     attitude.type_mask = 1|2|4; // Ignore body rates
 
+		// Retrieve parameters from Ctrl_param.yaml file
+		/*std::vector<double> waypoints;		
+		nh.getParam("sim/waypoints",waypoints);		
+
     // Need to send pose commands until throttle has been established
-    buff_pose.pose.position.x = 0.0;
+    buff_pose.pose.position.x = waypoints(1);
-    buff_pose.pose.position.y = 0.0;
+    buff_pose.pose.position.y = waypoints(2);
-    buff_pose.pose.position.z = 4.5;
+    buff_pose.pose.position.z = waypoints(3);*/
 
     // Desired mode is offboard
     mavros_msgs::SetMode offb_set_mode;

src/ref_signalGen.py

@@ -15,7 +15,7 @@ from oscillation_ctrl.msg import tethered_status, RefSignal
 
 from controller_msgs.msg import FlatTarget
 
-from geometry_msgs.msg import Pose, Vector3, PoseStamped 
+from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point 
 
 from geometry_msgs.msg import TwistStamped
 from sensor_msgs.msg import Imu
@@ -58,6 +58,7 @@ class Main:
 		self.has_run	= 0		# Bool to keep track of first run instance
 		self.drone_id = ''
 		self.pload_id = ''
+
 		self.pl_pos 	= Pose()		
 		self.dr_pos 	= Pose()
 		self.dr_vel		= self.vel_data.twist.linear
@@ -76,7 +77,7 @@ class Main:
 		self.EPS_I 		= np.zeros(9)		 # Epsilon shapefilter
 		
 		# Constants for smooth path generation
-		self.w_tune		= 3.0 # 3.13 works well? #########################################################################
+		self.w_tune		= 3.13 # 3.13 works well? #########################################################################
 		self.epsilon	= 0.7 								# Damping ratio
 		self.wn				= math.sqrt(9.81/self.tetherL)
 		self.wd				= self.wn*math.sqrt(1 - self.epsilon**2)
@@ -94,7 +95,15 @@ class Main:
 		self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T
 
  		# Desired position array
-		self.xd = np.array([[0],[0],[5.0]])
+		#if rospy.has_param('sim/waypoints'):
+		#	self.xd	= rospy.get_param('sim/waypoints') # waypoints
+		#elif rospy.get_time() - self.tstart >= 3.0:
+		#	self.xd = np.array([[0],[0],[5.0]]) # make our own if there are no waypoints
+
+		self.xd = Point()
+		self.get_xd = rospy.ServiceProxy('status/waypoint_tracker',WaypointTrack)		
+
+
 		self.des_waypoints = True # True = changing waypoints
 
 		# Initial conditions: [pos, vel, acc, jerk]
@@ -148,7 +157,7 @@ class Main:
 		rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
 		rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
 		rospy.Subscriber('/mavros/state', State, self.state_cb)
-		rospy.Subscriber('/reference/waypoints',PoseStamped, self.waypoints_cb)
+		#rospy.Subscriber('/reference/waypoints',Position, self.waypoints_cb) ### Change to position
 
 # --------------------------------------------------------------------------------#		
 # 																	PUBLISHERS
@@ -220,15 +229,15 @@ class Main:
 		except ValueError:
 			pass
 
-	def waypoints_cb(self,msg):
+	def waypoints_srv_cb(self):
+		rospy.wait_for_service('status/waypoint_tracker')
 		try:
-			if self.des_waypoints == True:
+			self.xd = self.get_xd(True)
-				self.xd[0] = msg.pose.position.x
+#			if self.des_waypoints == True:
-				self.xd[1] = msg.pose.position.y
-				#self.xd[2] = msg.pose.position.z
 
 		except ValueError:
 			pass
+
 # --------------------------------------------------------------------------------#		
 # 																			ALGORITHM
 # --------------------------------------------------------------------------------#

src/step.py

@@ -4,7 +4,7 @@
 ### Generate step input in x or y direction
 import rospy,  tf
 import time
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Point
 from std_msgs.msg import Bool
 
 class Main:
@@ -12,7 +12,7 @@ class Main:
 	def __init__(self):
 
 		# variable(s)
-		self.Point 	= PoseStamped()
+		self.Point 	= Point()
 		# Init x, y, & z coordinates
 		self.Point.pose.position.x	= 1
 		self.Point.pose.position.y	= 0
@@ -24,7 +24,7 @@ class Main:
 		rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) 
 
 		# publisher(s), with specified topic, message type, and queue_size
-		self.pub_step = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5) 	 	
+		self.pub_step = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) 	 	
 
 		# rate(s)
 		pub_rate = 1 # rate for the publisher method, specified in Hz

src/wpoint_tracker.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python2.7
+
+### Cesar Rodriguez March 2022
+### Reads waypoints from .yaml file and keeps track of them
+
+import rospy,  tf
+import rosservice
+import time
+from geometry_msgs.msg import Point,Pose
+from oscillation_ctrl.srv import WaypointTrack,WaypointTrackResponse
+
+class Main:
+
+	def __init__(self):
+
+		# Variables needed for testing start 
+		self.tstart		= rospy.get_time() # Keep track of the start time
+		while self.tstart == 0.0:				 # Need to make sure get_rostime works
+			self.tstart		= rospy.get_time()
+	
+		# initialize variables
+		self.xd = Point() 			# used to in response
+		self.wpoints = []				# used to save waypoint from yaml file
+		self.resp = WaypointTrackResponse() 
+		self.param_exists = False # check in case param does not exist
+
+		while self.param_exists == False:
+			if rospy.has_param('sim/waypoints'):
+				self.wpoints	= rospy.get_param('sim/waypoints') # get waypoints
+				self.xd.x = self.wpoints['x']
+				self.xd.y = self.wpoints['y']
+				self.xd.z = self.wpoints['z']
+				self.param_exists = True
+
+			elif rospy.get_time() - self.tstart >= 3.0:
+				self.xd.x = 0.0
+				self.xd.y = 0.0
+				self.xd.z = 5.0
+
+		# service(s)
+
+
+		# subscriber(s)	
+		rospy.Subscriber('/reference/waypoints',Point, self.waypoints_cb)		
+		
+		# publisher(s)
+
+
+	def waypoint_relay(self,req):
+		self.resp.xd = self.xd
+		return self.resp				
+
+	# Change desired position if there is a change
+	def waypoints_cb(self,msg):
+		try:
+			self.xd = msg
+		except ValueError or TypeError:
+			pass
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('waypoints_server',anonymous=False)
+	try:
+		obj = Main() # create class object
+		s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay)	
+		rospy.spin() # loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass
+

srv/WaypointTrack.srv

@@ -0,0 +1,4 @@
+bool query			# bool to request waypoints
+---
+geometry_msgs/Point xd 	# waypoints
+