81 lines
2.3 KiB
Python
81 lines
2.3 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
|
||
|
|
from std_msgs.msg import Bool
|
||
|
|
from oscillation_ctrl.srv import UseCtrl, UseCtrlResponse
|
||
|
|
|
||
|
|
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.use_ctrl = None
|
||
|
|
self.ctrl_state = UseCtrlResponse()
|
||
|
|
|
||
|
|
for i in range(3):
|
||
|
|
try:
|
||
|
|
self.use_ctrl = rospy.get_param('/status/change_mode')
|
||
|
|
except:
|
||
|
|
rospy.loginfo('Waiting for parameter "status/change_mode" to become available...')
|
||
|
|
rospy.loginfo('Trying {0} more time(s)'.format(2-i))
|
||
|
|
rospy.sleep(0.1)
|
||
|
|
|
||
|
|
if self.use_ctrl is None:
|
||
|
|
raise rospy.ROSInitException
|
||
|
|
|
||
|
|
# service(s)
|
||
|
|
rospy.Service('/status/att_ctrl_srv', UseCtrl, self.ctrl_tracker)
|
||
|
|
|
||
|
|
# subscriber(s)
|
||
|
|
|
||
|
|
# publisher(s)
|
||
|
|
self.state_pub = rospy.Publisher('/status/att_ctrl', Bool, queue_size=10)
|
||
|
|
rospy.Timer(rospy.Duration(1.0/2.0), self.publisher)
|
||
|
|
|
||
|
|
def ctrl_tracker(self, request):
|
||
|
|
""" Keeps track of controller state """
|
||
|
|
# check if we want to change state or whether it is a check
|
||
|
|
if request.query:
|
||
|
|
# change use_ctrl param to requested state
|
||
|
|
rospy.set_param('/status/use_ctrl', request.state)
|
||
|
|
|
||
|
|
# set result to new state
|
||
|
|
self.ctrl_state.result = rospy.get_param('/status/use_ctrl')
|
||
|
|
|
||
|
|
# check if everything worked
|
||
|
|
if request.state is self.ctrl_state.result:
|
||
|
|
self.ctrl_state.success = True
|
||
|
|
else:
|
||
|
|
self.ctrl_state.success = False
|
||
|
|
else:
|
||
|
|
self.ctrl_state.success = True
|
||
|
|
return self.ctrl_state
|
||
|
|
|
||
|
|
def publisher(self, timer):
|
||
|
|
""" publish state so that nodes do not have query the service every time"""
|
||
|
|
self.state_pub.publish(self.ctrl_state.result)
|
||
|
|
|
||
|
|
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
|
||
|
|
|