Added waypoint_client

.gitignore

@@ -2,7 +2,8 @@ launch/debug.launch
 launch/cortex_bridge.launch
 src/MoCap_Localization_*.py
 src/Mocap_*.py
-src/*_client.py
+src/killswitch_client.py
+src/land_client.py
 msg/Marker.msg
 msg/Markers.msg
 *.rviz

src/waypoint_client.py

@@ -0,0 +1,49 @@
+#!/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 oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
+
+class Main:
+	def __init__(self):
+		self.request = WaypointTrackRequest()
+		self.request.query = True # set to true to set new waypoints
+		self.get_xd = WaypointTrackRequest() # need to get desired height
+		# service(s)
+		self.waypoint_cl = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
+
+	def user_input(self):
+		self.request.xd.x = input('Enter desired pos in x dir: ')
+		self.request.xd.y = input('Enter desired pos in y dir: ')
+
+	def new_waypoint(self):
+
+		rospy.wait_for_service('/status/waypoint_tracker')
+		self.user_input()
+		try:
+
+			# get xd.z
+			resp = self.waypoint_cl(self.get_xd)
+			self.request.xd.z = resp.xd.z
+			return self.waypoint_cl(self.request)
+
+		except rospy.ServiceException as e:
+			print('Service call failed: %s'%e)
+			
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('waypoint_client',anonymous=True)
+	try:
+		service = Main() 				# create class object
+		xd = service.new_waypoint()
+		print(xd)
+		rospy.spin() 	# loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass