oscillation_ctrl/src/wpoint_tracker.py

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