diff --git a/.gitignore b/.gitignore index 6e83146..3dbf71b 100644 --- a/.gitignore +++ b/.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 diff --git a/src/waypoint_client.py b/src/waypoint_client.py new file mode 100755 index 0000000..350e15e --- /dev/null +++ b/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