def __init__(self):
# Instantiate a "mavros/setpoint_raw/local" message
self.sp = PositionTarget()
self.sp.type_mask = int('010111111000', 2) # set the flag to use position setpoints and yaw angle
self.sp.coordinate_frame = 1 # FRAME_LOCAL_NED
# Instantiate a "mavros/setpoint_raw/global" message
self.sp_glob = GlobalPositionTarget()
self.sp_glob.type_mask = int('111111111000', 2)
# self.sp_glob.type_mask = 64
self.sp_glob.coordinate_frame = 11 # FRAME_GLOBAL_REL_INT
self.sp_glob.latitude = 0
self.sp_glob.longitude = 0
self.sp_glob.altitude = 0
# Instantiate a Target_GPS message
self.target_gps = Target_GPS()
# A Message for the current local position of the drone
self.local_pos = Point(0.0, 0.0, 0.0)
self.global_pos = Point(0.0, 0.0, 0.0)
# initial values for setpoints
self.sp.position.x = 0.0 # geometry_msgs/Point position
self.sp.position.y = 0.0
# Record Drone's GPS
self.drone_GPS_record = []
# Record Target's GPS
self.target_GPS_record = []
from geometry_msgs.msg import Point, PoseStamped # /opt/ros/melodic/share/geometry_msgs
# import all mavros messages and services
from mavros_msgs.msg import *
from mavros_msgs.srv import *
from sensor_msgs.msg import NavSatFix
# import a message from Raspberry pi CAMERA
# Message name: /target_msg
from targetDetection_topic.msg import TargetPosition
# import a message from Raspberry pi GPS
# Message name: /Target_GPS_msg
from GPS_topic.msg import Target_GPS
# Instantiate a Target_GPS message
target_gps = Target_GPS()
# A Message for the current local position of the drone
global_pos = Point(0.0, 0.0, 0.0)
absolute_altitude = 0
class Controller:
# initialization method
def __init__(self):
pass
# Callbacks
def GPS_callback(self, msg):
global target_gps
target_gps.latitude = msg.latitude