72 lines
1.8 KiB
Python
72 lines
1.8 KiB
Python
|
#!/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
|
||
|
|
|