forked from cesar.alejandro/oscillation_ctrl
84 lines
2.4 KiB
Python
Executable File
84 lines
2.4 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
|
|
import math
|
|
from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
|
|
from sensor_msgs.msg import NavSatFix
|
|
|
|
class Waypoint:
|
|
def __init__(self,latitude,longitude):
|
|
self.lat = latitude
|
|
self.lon = longitude
|
|
|
|
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
|
|
self.has_run = False
|
|
self.waypoints = []
|
|
self.R = 6371e3 # Earth radius in meters
|
|
self.global_home = NavSatFix()
|
|
|
|
# service(s)
|
|
self.waypoint_cl = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
|
|
|
# subscribers
|
|
rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.globalPos_cb)
|
|
|
|
def globalPos_cb(self,msg):
|
|
if not self.has_run: # Will not run after global home has been set
|
|
if msg.latitude != 0.0 and msg.longitude != 0.0:
|
|
self.gl_home = Waypoint(msg.latitude,msg.longitude) # Save global home
|
|
self.has_run = True
|
|
|
|
def run(self):
|
|
self.wp_input()
|
|
self.determine_xd(self.wp)
|
|
self.set_waypoint()
|
|
|
|
def wp_input(self):
|
|
# testing:
|
|
self.lat = input('Enter lat: ')
|
|
self.lon = input('Enter lon: ')
|
|
|
|
self.lat = 44.6480876
|
|
self.lon = -63.5782662
|
|
self.wp = Waypoint(self.lat,self.lon)
|
|
|
|
def determine_xd(self,wp):
|
|
# https://en.wikipedia.org/wiki/Equirectangular_projection
|
|
self.request.xd.x = self.R*(wp.lon - self.gl_home.lon)*(math.pi/180)*math.cos((wp.lat + self.gl_home.lat)*(math.pi/360))
|
|
self.request.xd.y = self.R*(wp.lat - self.gl_home.lat)*(math.pi/180)
|
|
"""
|
|
360 in xd.x because (wp.lat + self.gl_home.lat)/2
|
|
"""
|
|
|
|
def set_waypoint(self):
|
|
rospy.wait_for_service('/status/waypoint_tracker')
|
|
try:
|
|
resp = self.waypoint_cl(self.get_xd) # get xd.z
|
|
self.request.xd.z = resp.xd.z
|
|
self.waypoint_cl(self.request)
|
|
rospy.loginfo(self.waypoint_cl(self.get_xd)) # want to return value to give feedback to user?
|
|
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
|
|
service.run()
|
|
rospy.spin() # loop until shutdown signal
|
|
|
|
except rospy.ROSInterruptException:
|
|
pass
|