78 lines
2.0 KiB
Python
Executable File
78 lines
2.0 KiB
Python
Executable File
#!/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
|
|
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('status/waypoints'):
|
|
self.wpoints = rospy.get_param('status/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 >= 5.0:
|
|
self.xd.x = 0.0
|
|
self.xd.y = 0.0
|
|
self.xd.z = 1.5
|
|
break
|
|
|
|
# service(s)
|
|
|
|
|
|
# subscriber(s)
|
|
# rospy.Subscriber('/reference/waypoints',Point, self.waypoints_cb)
|
|
|
|
# publisher(s)
|
|
|
|
def waypoint_relay(self,req):
|
|
#TODO Need to add check to make sure query is boolean
|
|
if req.query:
|
|
self.xd = req.xd
|
|
self.resp.xd = self.xd
|
|
else:
|
|
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.loginfo_once('waypoints_server has started with:\nxd.x = %.1f\nxd.y = %.1f\nxd.z = %.1f', obj.xd.x, obj.xd.y, obj.xd.z)
|
|
|
|
rospy.spin() # loop until shutdown signal
|
|
|
|
except rospy.ROSInterruptException:
|
|
pass
|
|
|