def mission_create (self):
param = rospy.get_param('~waypoints')
rospy.loginfo('Waypoints from parameter server: %s', param)
# rospy.logdebug(param)
wl = WaypointList()
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = param[0]['alt'] # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = 0
wp.y_long = 0
wp.z_alt = param[0]['alt']
wl.waypoints.append(wp)
for index in range(len(param)):
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = param[index]['lat']
wp.y_long = param[index]['lon']
wp.z_alt = param[index]['alt']
wl.waypoints.append(wp)
wp = Waypoint()
wp.frame = 2
wp.command = MAV_CMD_RTL
wp.is_current = False
wp.autocontinue = True
wl.waypoints.append(wp)
rospy.loginfo('Waypoints %s', wl)
self.push_mission(wl.waypoints)
self.arming()
rospy.sleep(2)
self.set_current(0)
# def gps_cb(data):
# lat = data.latitude
# lon = data.longitude
# rospy.Subscriber("mavros/global_position/global", NavSatFix, gps_cb)
rospy.spin()
def main_control():
#pdb.set_trace()
rospy.init_node('offb_node', anonymous=True)
pub = rospy.Publisher("contoller", control, queue_size=10)
while True:
n, lat, lon, alt = raw_input().split()
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL_REL_ALT
wp.command = CommandCode.NAV_WAYPOINT
wp.is_current = True
wp.autocontinue = True
wp.param1 = 0
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = alt
cont = control()
cont.waypoint = wp
cont.uav_no = n
pub.publish(cont)
def parse_file(self):
file = open("/home/clement/catkin_ws/src/ros_ulysse/src/test.txt", "r")
L = file.readlines()
nb_wp = len(L)
print("Nombre de waypoints :", nb_wp)
self.req = WaypointPushRequest()
for i in range(len(L) - 1):
ligne = L[i + 1].split("\t")
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_WAYPOINT #Navigate to Waypoint
wp.is_current = bool(ligne[1])
wp.autocontinue = bool(ligne[2])
wp.param1 = float(ligne[4])
wp.param2 = float(ligne[5])
wp.param3 = float(ligne[6])
wp.param4 = float(ligne[7])
wp.x_lat = float(ligne[8])
wp.y_long = float(ligne[9])
wp.z_alt = float(ligne[10])
self.waypoints_list.append(wp)
self.req.waypoints = self.waypoints_list
print(type(self.req.waypoints))
def takeoff_waypoint(curr_count, new_latitude, new_longitude):
waypoint_clear_client(curr_count)
wl = WaypointList()
with open(path_to_waypoint_file) as waypoints:
lines = waypoints.readlines()
for line in lines[1:]:
values = list(line.split('\t'))
wp = Waypoint()
wp.is_current = bool(values[1])
wp.frame = int(values[2])
wp.command = int(values[3])
wp.param1 = float(values[4])
wp.param2 = float(values[5])
wp.param3 = float(values[6])
wp.param4 = float(values[7])
# print(latitude, longitude, "xxxxxx", new_latitude, new_longitude)
wp.x_lat = float(new_latitude)
wp.y_long = float(new_longitude)
wp.z_alt = float(values[10])
wp.autocontinue = bool(values[11])
wl.waypoints.append(wp)
# print(wl.waypoints)
try:
rospy.wait_for_service('vehicle' + str(curr_count) +
'/mavros/mission/push')
service = rospy.ServiceProxy(
'vehicle' + str(curr_count) + '/mavros/mission/push', WaypointPush)
if service.call(0, wl.waypoints).success:
print('write mission success')
else:
print('write mission error')
except:
print("Service call failed: %s" % e)
def createWaypointPush(gps_list):
clearCurrentMission()
waypointList = WaypointList()
# waypointList.start_index =
for coord in gps_list:
wp = Waypoint()
wp.frame = 3
wp.command = 16 #simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 #takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = coord.latitude
wp.y_long = coord.longitude
wp.z_alt = coord.altitude
waypointList.waypoints.append(wp)
for wp in waypointList.waypoints:
print(wp.x_lat, wp.y_long, wp.z_alt)
try:
pushMissionSrv = rospy.ServiceProxy('/mavros/mission/push',
WaypointPush)
pushResponse = pushMissionSrv(0, waypointList.waypoints)
rospy.loginfo(pushResponse)
except rospy.ServiceException, e:
rospy.loginfo('Service call failed: %s' % e)
def get_mission(mission_num=0):
rospack = rospkg.RosPack()
package_path = rospack.get_path('snotbot')
tree = ET.parse(package_path + '/scripts/missions.xml')
for mission in tree.getroot():
if mission.attrib['id'] == mission_num:
break
waypoint_list = []
for item in mission:
waypoint = Waypoint()
waypoint.frame = int(item.find('frame').text)
waypoint.command = int(item.find('command').text)
waypoint.is_current = bool(item.find('is_current').text)
waypoint.autocontinue = bool(item.find('autocontinue').text)
waypoint.param1 = float(item.find('param1').text)
waypoint.param2 = float(item.find('param2').text)
waypoint.param3 = float(item.find('param3').text)
waypoint.param4 = float(item.find('param4').text)
waypoint.x_lat = float(item.find('latitude').text)
waypoint.y_long = float(item.find('longitude').text)
waypoint.z_alt = float(item.find('altitude').text)
waypoint_list.append(waypoint)
return waypoint_list
def update_waypoints(self):
'''
Creates and publishes waypoint list using mavros/waypoints, also update position of model (marker) in Gazebo
:return:
'''
self.waypoint_msg = WaypointList()
# Publish mavros/waypoints
way_tmp = Waypoint()
way_tmp.frame = 0
way_tmp.command = 16
way_tmp.is_current = True
self.waypoint_msg.waypoints.append(way_tmp)
way_tmp = Waypoint()
way_tmp.frame = 0
way_tmp.command = 22
way_tmp.is_current = False
way_tmp.autocontinue = True
way_tmp.param2 = 1
way_tmp.param3 = 0
way_tmp.param4 = 0
way_tmp.x_lat = self.current_waypoint_position_x
way_tmp.y_long = self.current_waypoint_position_y
way_tmp.z_alt = 0
self.waypoint_msg.waypoints.append(way_tmp)
self.mavros_waypoint_pub.publish(self.waypoint_msg)
self.update_waypoint_model()
def parse_file(self):
# file_name=input("Entrer le nom du fichier: ")
file = open("../sources/txt_mission_planner/ulysse_promenade.txt", "r")
L = file.readlines()
nb_wp = len(L)
print("Nombre de waypoints :", nb_wp - 1)
self.req = WaypointPushRequest()
for i in range(1, len(L)):
ligne = L[i].split("\t")
print(ligne)
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_WAYPOINT #Navigate to Waypoint
wp.is_current = True
wp.autocontinue = False
wp.param1 = float(ligne[4])
wp.param2 = float(ligne[5])
wp.param3 = float(ligne[6])
wp.param4 = float(ligne[7])
wp.x_lat = float(ligne[8])
wp.y_long = float(ligne[9])
wp.z_alt = float(ligne[10])
self.waypoints_list.append(wp)
self.req.waypoints = self.waypoints_list
print(self.req.waypoints)
def addWaypoint(lat, lon, obstacles=False, detour=False, override=False):
"""Send waypoint from Odroid to Pixhawk.
Keyword arguments:
detour -- insert waypoint at beginning of array
override -- clear previous waypoints)
"""
waypoint = Waypoint()
waypoint.frame = 3
waypoint.command = 16
waypoint.is_current = 0
waypoint.autocontinue = True
waypoint.param1 = 0 #hold time
waypoint.param2 = 2 #acceptance radius (m)
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = altitude
publish("Adding waypoint: ("+str(waypoint.x_lat)+", "+str(waypoint.y_long)+")")
oldWaypoints = waypoints[:]
publish("Old waypoints: " + str(len(oldWaypoints)))
if override: result = push([waypoint])
elif detour: result = push([waypoint]+oldWaypoints)
else: result = push(oldWaypoints + [waypoint])
publish("Result: " + str(result))
def waypoint_push_client():
waypoint_clear_client()
wl = WaypointList()
path_to_way = dir_path = os.path.dirname(os.path.realpath(__file__))
with open(path_to_way + '/way.txt', 'r') as wps:
lines = wps.readlines()
for line in lines[1:]:
values = list(line.split('\t'))
wp = Waypoint()
wp.is_current = bool(values[1])
wp.frame = int(values[2])
wp.command = int(values[3])
wp.param1 = float(values[4])
wp.param2 = float(values[5])
wp.param3 = float(values[6])
wp.param4 = float(values[7])
wp.x_lat = float(values[8])
wp.y_long = float(values[9])
wp.z_alt = float(values[10])
wp.autocontinue = bool(values[11])
wl.waypoints.append(wp)
# wp.frame = 3
# wp.command = 22 # takeoff
# wp.is_current = True
# wp.autocontinue = True
# wp.param1 = data[0]['altitude'] # takeoff altitude
# wp.param2 = 0
# wp.param3 = 0
# wp.param4 = 0
# wp.x_lat = data[0]['latitude']
# wp.y_long = data[0]['longitude']
# wp.z_alt = data[0]['altitude']
#wl.waypoints.append(wp)
# for point in data:
# wp = Waypoint()
# wp.frame = 3
# wp.command = 16 # simple point
# wp.is_current = False
# wp.autocontinue = True
# wp.param1 = 0 # takeoff altitude
# wp.param2 = 0
# wp.param3 = 0
# wp.param4 = 0
# wp.x_lat = point['latitude']
# wp.y_long = point['longitude']
# wp.z_alt = point['altitude']
# wl.waypoints.append(wp)
try:
service = rospy.ServiceProxy('mavros/mission/push', WaypointPush)
if service.call(0, wl.waypoints).success:
print 'write mission success'
else:
print 'write mission error'
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def MR_Photo_Distance_Waypoint(distance):
#set photo-distance (not defined in MissionControl-Node)
#mavLink sets this as separated Message of type "Waypoint" but with command "206" - "DO_SET_CAM_TRIGG_DIST"
mrWaypoint = Waypoint()
mrWaypoint.command = 206 #DO_SET_CAM_TRIGG_DIST
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.param1 = distance #Distance, 0 to stop taking Photos
mrWaypoint.param2 = 0 #"1" => triger camera once
return(mrWaypoint)
def mission_create():
param = rospy.get_param('waypoints')
print 'Waypoints from parameter server: '
print param
wl = WaypointList()
wp = Waypoint()
wp.frame = MAV_GLOBAL_FRAME
wp.command = MAV_CMD_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = param[0]['alt'] # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = param[0]['alt']
wl.waypoints.append(wp)
for index in range(len(param)):
wp = Waypoint()
wp.frame = MAV_GLOBAL_FRAME
wp.command = MAV_CMD_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = param[index]['lat']
wp.y_long = param[index]['lon']
wp.z_alt = param[index]['alt']
wl.waypoints.append(wp)
wp = Waypoint()
wp.frame = 2
wp.command = MAV_CMD_RTL
wp.is_current = False
wp.autocontinue = True
wl.waypoints.append(wp)
push_mission(wl.waypoints)
def mission_create(mission_msg):
rospy.loginfo('Waypoints from message server: ')
rospy.loginfo(mission_msg)
wl = WaypointList()
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = mission_msg.waypoints[0].altitude # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = mission_msg.waypoints[0].altitude
wl.waypoints.append(wp)
for item in mission_msg.waypoints:
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = item.latitude
wp.y_long = item.longitude
wp.z_alt = item.altitude
wl.waypoints.append(wp)
wp = Waypoint()
wp.frame = 2
wp.command = self.MAV_CMD_RTL
wp.is_current = False
wp.autocontinue = True
wl.waypoints.append(wp)
push_mission(wl.waypoints)
def mission_create(mission_msg):
rospy.loginfo('Waypoints from message server: ')
rospy.loginfo(mission_msg)
wl = WaypointList()
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = mission_msg.waypoints[0].altitude # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = self.lat
wp.y_long = self.lon
wp.z_alt = mission_msg.waypoints[0].altitude
wl.waypoints.append(wp)
for item in mission_msg.waypoints:
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = item.latitude
wp.y_long = item.longitude
wp.z_alt = item.altitude
wl.waypoints.append(wp)
# wp = Waypoint()
# wp.frame = 2
# wp.command = CommandCode.NAV_RETURN_TO_LAUNCH
# wp.is_current = False
# wp.autocontinue = True
# wl.waypoints.append(wp)
push_mission(wl.waypoints)
def waypoint(self, coordinates): #coordinates = [lat, long, alt]
####################################### IMPLEMENT SECURITY FOR ALTITUDE (CHECK WITH THE GROUND ALT) ####################################
global waypointFlag
# Reset waypointFlag
waypointFlag = False
counter = 0
wpPushFlag = False
# Deliver a new mission plan to the autopilot
# waypoint
wp = Waypoint()
wp.frame = 3 # Global frame:0 with relative alt: 3
wp.command = 16 # Navigate to waypoint
wp.is_current = True # TBC
wp.autocontinue = True # TBC
wp.param1 = 5 # Hold time is sec
wp.param2 = 0 # Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)
wp.param3 = 0 # 0 to pass through WP, else radius in meters to pass by WP
wp.param4 = float(
'nan') # Desired yaw angle at waypoint (NaN for unchanged)
wp.x_lat = coordinates[0]
wp.y_long = coordinates[1]
wp.z_alt = coordinates[2]
# Push new waypoint
while (wpPushFlag == False) and (counter <= 3):
rospy.loginfo("Request new mission plan")
rospy.wait_for_service('/mavros/mission/push')
try:
pushWaypointsRequest = rospy.ServiceProxy(
'/mavros/mission/push', WaypointPush)
pushWaypointsResponse = pushWaypointsRequest(start_index=0,
waypoints=[wp])
rospy.loginfo(pushWaypointsResponse)
wpPushFlag = pushWaypointsResponse.success
print("TakeOff Pushed")
except rospy.ServiceException as e:
rospy.loginfo("Request new mission plan failed: %s" % e)
counter += 1
# Message?
# if not in AUTO, set it into this mode
if self.state[3] != 'AUTO':
self.setFlightMode(mode='AUTO')
print("Okay it should be AUTO now")
while waypointFlag == False: # Is True when the WP is reached
# print("This is the waypoint %s" %waypointFlag)
pass
def takeOff(self):
'Automatic TAKEOFF'
global waypointFlag
# Reset waypointFlag
waypointFlag = False
counter = 0
wpPushFlag = False
# Deliver a new mission plan to the autopilot
# waypoint
wp = Waypoint()
wp.frame = 3 # Global frame:0 with relative alt: 3
wp.command = 22 # Navigate to waypoint
wp.is_current = True # TBC
wp.autocontinue = True # TBC
wp.param1 = 5 # Pitch
wp.param2 = 0 # Empty
wp.param3 = 0 # Empty
wp.param4 = float(
'nan') # Desired yaw angle at waypoint (NaN for unchanged)
wp.x_lat = 0
wp.y_long = 0
wp.z_alt = 10
# Push new waypoint
while (wpPushFlag == False) and (counter <= 3):
rospy.loginfo("Request new mission plan")
rospy.wait_for_service('/mavros/mission/push')
try:
pushWaypointsRequest = rospy.ServiceProxy(
'/mavros/mission/push', WaypointPush)
pushWaypointsResponse = pushWaypointsRequest(start_index=0,
waypoints=[wp])
rospy.loginfo(pushWaypointsResponse)
wpPushFlag = pushWaypointsResponse.success
print("Waypoint Pushed")
except rospy.ServiceException as e:
rospy.loginfo("Request new mission plan failed: %s" % e)
counter += 1
# Message?
# if not in AUTO, set it into this mode
if self.state[3] != 'AUTO':
self.setFlightMode(mode='AUTO')
print("Okay it should be AUTO now")
while waypointFlag == False: # Is True when the WP is reached
# print("This is the waypoint %s" %waypointFlag)
pass
'''
def MR_Speed_Waypoint(speed):
#set Speed used for Path to this new Waypoint
#mavLink sets this as separated Message of type "Waypoint" but with command "178" - "DO_CHANGE_SPEED"
mrWaypoint = Waypoint()
mrWaypoint.command = 178 #DO_CHANGE_SPEED
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.param1 = 1 #Ground Speed, DJI drones are capable of this
mrWaypoint.param2 = speed
mrWaypoint.param3 = -1 #unchanged (no) throttle-Value
mrWaypoint.param4 = 0 #absolute value
return(mrWaypoint)
def waypoint(lat, lon, alt, delay):
w = Waypoint()
w.frame = Waypoint.FRAME_GLOBAL
w.command = CommandCode.NAV_WAYPOINT
w.is_current = False
w.autocontinue = True
w.param1 = delay # Hold time in mession
w.param2 = 2 # Position trashold in meters
w.x_lat = lat
w.y_long = lon
w.z_alt = alt
return w
def waypoint(lat, lon, alt, delay):
w = Waypoint()
w.frame = Waypoint.FRAME_GLOBAL
w.command = CommandCode.NAV_WAYPOINT
w.is_current = False
w.autocontinue = True
w.param1 = delay # Hold time in mession
w.param2 = 2 # Position trashold in meters
w.x_lat = lat
w.y_long = lon
w.z_alt = alt
return w
def waypoint(lat, lon, alt, delay):
w = Waypoint()
w.frame = 3 # Currently is relative to HOME frame TODO: set global frame
w.command = CommandCode.NAV_WAYPOINT
w.is_current = False
w.autocontinue = True
w.param1 = delay # Hold time in mession
w.param2 = 2 # Position trashold in meters
w.x_lat = lat
w.y_long = lon
w.z_alt = alt
return w
def mission_waypoint_push(self, request, response):
rospy.loginfo(request)
mission_push_service = rospy.ServiceProxy(
self.namespace + '/mavros/mission/push', WaypointPush)
waypoints = []
for i in range(2):
takeoff_waypoint = Waypoint()
takeoff_waypoint.command = 22
takeoff_waypoint.param1 = 0
takeoff_waypoint.param2 = 0
takeoff_waypoint.param3 = 0
takeoff_waypoint.param4 = 0
if self.location:
takeoff_waypoint.x_lat = self.location.latitude
takeoff_waypoint.y_long = self.location.longitude
takeoff_waypoint.z_alt = 10
takeoff_waypoint.is_current = True
takeoff_waypoint.autocontinue = True
takeoff_waypoint.frame = 3
waypoints.append(takeoff_waypoint)
for waypoint in request.get('waypoints'):
new_waypoint = Waypoint()
new_waypoint.command = waypoint.get('command')
new_waypoint.is_current = waypoint.get('is_current')
new_waypoint.autocontinue = waypoint.get('autocontinue')
new_waypoint.param1 = waypoint.get('param1')
new_waypoint.param2 = waypoint.get('param2')
new_waypoint.param3 = waypoint.get('param3')
new_waypoint.param4 = waypoint.get('param4')
new_waypoint.x_lat = waypoint.get('x_lat')
new_waypoint.y_long = waypoint.get('y_long')
new_waypoint.z_alt = waypoint.get('z_alt')
new_waypoint.frame = waypoint.get('frame')
waypoints.append(new_waypoint)
rospy.loginfo(waypoints)
local_response = mission_push_service(request.get('start_index'),
waypoints)
response['success'] = local_response.success
response['wp_transfered'] = local_response.wp_transfered
return local_response.success
def createLand():
wp = Waypoint()
wp.frame = 3
wp.command = 21
wp.autocontinue = True
wp.param1 = 25.0
wp.param2 = 3.0
wp.param3 = -0.0
wp.param4 = 0.0
wp.x_lat = 47.3667049
wp.y_long = 8.5499866
wp.z_alt = 10.0
return wp
def createWaypoint(_visitationRadius, _lat, _lon, _alt):
wp = Waypoint()
wp.frame = 3
wp.command = 16
wp.autocontinue = True
wp.param1 = 0
wp.param2 = _visitationRadius
wp.param3 = 0
wp.param4 = 0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def createLand(_lat, _lon, _alt):
wp = Waypoint()
wp.frame = 3
wp.command = 21
wp.autocontinue = True
wp.param1 = 25.0
wp.param2 = 3.0
wp.param3 = -0.0
wp.param4 = 0.0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def createWaypoint(_visitationRadius, _lat, _lon, _alt):
wp = Waypoint()
wp.frame = 3
wp.command = 16
wp.autocontinue = True
wp.param1 = 0
wp.param2 = _visitationRadius
wp.param3 = 0
wp.param4 = 0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def createLand():
wp = Waypoint()
wp.frame = 3
wp.command = 21
wp.autocontinue = True
wp.param1 = 25.0
wp.param2 = 3.0
wp.param3 = -0.0
wp.param4 = 0.0
wp.x_lat = 47.3667049
wp.y_long = 8.5499866
wp.z_alt = 10.0
return wp
def wpts(x, y, z):
d = Waypoint()
d.frame = 3
d.command = 16
d.is_current = False
d.autocontinue = True
d.param1 = 0.000000
d.param2 = 0.000000
d.param3 = 0.000000
d.param4 = 0.000000
d.x_lat = x #-35.362521
d.y_long = y #149.170465
d.z_alt = z #70.000000
return [d]
def wp_copy(wp):
w = Waypoint()
w.command = wp.command
w.frame = wp.frame
w.is_current = wp.is_current
w.autocontinue = wp.autocontinue
w.x_lat = wp.x_lat
w.y_long = wp.y_long
w.z_alt = wp.z_alt
w.param1 = wp.param1
w.param2 = wp.param2
w.param3 = wp.param3
w.param4 = wp.param4
return w
def MR_Gimbal_Pitch_Waypoint(pitch):
#set camera gimbal pitch to param
#mavLink sets this as separated Message of type "Waypoint" but with command "205" - "DO_MOUNT_CONTROL"
mrWaypoint = Waypoint()
mrWaypoint.command = 205 #DO_MOUNT_CONTROL
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.param1 = pitch #pitch
mrWaypoint.param2 = 0 #roll
mrWaypoint.param3 = 0 #yaw
mrWaypoint.param4 = 0 #altitude (not used in this mode)
mrWaypoint.x_lat = 0 #latitude (not used in this mode)
mrWaypoint.y_long = 0 #longitude (not used in this mode)
mrWaypoint.z_alt = 2 #MAV_MOUNT_MODE 2 => MAVLINK_TARGETING => "Start Mavlink Roll,Pitch,Yaw control with stabilization"
return(mrWaypoint)
def make_waypoint(frame, command, is_current, lat, lon, alt, hold):
waypoint = Waypoint()
waypoint.frame = frame
waypoint.command = command
waypoint.is_current = is_current
waypoint.autocontinue = True
waypoint.param1 = hold #hold time
waypoint.param2 = 0
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = alt
return waypoint
def make_global_waypoint(self, lat, lon, alt=DEFAULT_ALT, hold=5):
waypoint = Waypoint()
waypoint.frame = 3
waypoint.command = 16
waypoint.is_current = 0
waypoint.autocontinue = False
waypoint.param1 = hold #hold time
waypoint.param2 = 2
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = alt
return waypoint
def construct_waypoint(c_wp):
"""
Construct and return a proper mavros Waypoint structure
"""
the_wp = Waypoint()
the_wp.frame = 3
the_wp.command = 16
the_wp.is_current = 1
the_wp.autocontinue = 1
the_wp.param1 = 0
the_wp.param2 = 10
the_wp.param3 = 0
the_wp.param4 = 0
the_wp.x_lat = c_wp.latitude
the_wp.y_long = c_wp.longitude
the_wp.z_alt = ALTITUDE
return the_wp
def get_mission(mission_hash):
mission_url = 'http://{0}/ipfs/{1}'.format(os.environ['IPFS_HOST'],
mission_hash)
mission_data = urllib2.urlopen(mission_url).read()
mission_items = json.loads(mission_data)['items']
for item in mission_items:
w = Waypoint()
w.frame = item['frame']
w.command = item['command']
w.autocontinue = item['autoContinue']
w.param1 = item['param1']
w.param2 = item['param2']
w.param3 = item['param3']
w.param4 = item['param4']
w.x_lat = item['coordinate'][0]
w.y_long = item['coordinate'][1]
w.z_alt = item['coordinate'][2]
yield w
def createTakeoffCurr():
# 22 MAV_CMD_NAV_TAKEOFF Takeoff from ground / hand
# Mission Param #1 Minimum pitch (if airspeed sensor present), desired pitch without sensor
# Mission Param #2 Empty
# Mission Param #3 Empty
# Mission Param #4 Yaw angle (if magnetometer present), ignored without magnetometer
# Mission Param #5 Latitude
# Mission Param #6 Longitude
# Mission Param #7 Altitude
wp = Waypoint()
wp.frame = 3
wp.command = 22
wp.autocontinue = True
wp.param1 = 0.261799395084
wp.param2 = 0.0
wp.param3 = 0.0
wp.param4 = 0.0
wp.x_lat = 47.3669242859
wp.y_long = 8.54999923706
wp.z_alt = 10.0
return wp
def createTakeoffCurr(_lat, _lon, _alt):
# 22 MAV_CMD_NAV_TAKEOFF Takeoff from ground / hand
# Mission Param #1 Minimum pitch (if airspeed sensor present), desired pitch without sensor
# Mission Param #2 Empty
# Mission Param #3 Empty
# Mission Param #4 Yaw angle (if magnetometer present), ignored without magnetometer
# Mission Param #5 Latitude
# Mission Param #6 Longitude
# Mission Param #7 Altitude
wp = Waypoint()
wp.frame = 3
wp.command = 22
wp.autocontinue = True
wp.param1 = 0.261799395084
wp.param2 = 0.0
wp.param3 = 0.0
wp.param4 = 0.0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def waypointh(): #for home.
d = Waypoint()
d.frame = 0
d.command = 16
d.is_current = False
d.autocontinue = True
d.param1 = 0.000000
d.param2 = 0.000000
d.param3 = 0.000000
d.param4 = 0.000000
d.x_lat = -35.363262
d.y_long = 149.165238
d.z_alt = 584.090027
rospy.wait_for_service('/mavros/mission/push')
try:
wp = rospy.ServiceProxy('/mavros/mission/push', WaypointPush)
response = wp(0, [d])
response.success
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def update_waypoints(self):
"""
Creates and publishes waypoint list using mavros/waypoints, also update position of model (marker) in Gazebo
:return:
"""
# Publish mavros/waypoints
way_tmp = Waypoint()
way_tmp.frame = 0
way_tmp.command = 22
way_tmp.is_current = True
way_tmp.autocontinue = True
way_tmp.param2 = 1
way_tmp.param3 = 0
way_tmp.param4 = 0
way_tmp.x_lat = self.current_waypoint_position_x
way_tmp.y_long = self.current_waypoint_position_y
way_tmp.z_alt = self.current_waypoint_position_z
self.mavros_waypoint_pub.publish(way_tmp)
self.update_waypoint_model()
def callback(data):
global wp_plan
global waypoint_iter
global distance
global integrated_area
global integration_started
global vasi_mikri
global pr_closest_point
actual_position = waypoint(data.latitude, data.longitude)
previous_wp = wp_plan[waypoint_iter]
next_wp = wp_plan[waypoint_iter + 1]
closest_point = get_closest_point_on_segment(previous_wp.latitude,
previous_wp.longitude,
next_wp.latitude,
next_wp.longitude,
data.latitude, data.longitude)
# if waypoint_iter > 0:
# if integration_started == 0:
# rospy.loginfo("========= INTEGRATION STARTED =========")
# vasi_mikri = get_distance(data.latitude, data.longitude, closest_point.latitude, closest_point.longitude)
# pr_closest_point = waypoint(latitude=closest_point.latitude, longitude=closest_point.longitude)
# integration_started = 1
# else:
# vasi_megali = get_distance(data.latitude, data.longitude, closest_point.latitude, closest_point.longitude)
# integrated_area += ((vasi_mikri + vasi_megali) * get_distance(pr_closest_point.latitude, pr_closest_point.longitude,
# closest_point.latitude, closest_point.longitude)) / 2
# vasi_mikri = vasi_megali
# pr_closest_point = closest_point
bearing = find_bearing(previous_wp, next_wp)
# the next is the second way of calculating the next waypoint, by including the distance from the actual position of the UAV
#remaining_distance = distance - get_distance(data.latitude, data.longitude, closest_point.latitude, closest_point.longitude)
#c_wp = find_command_waypoint(remaining_distance, bearing, closest_point)
c_wp = find_command_waypoint(distance, bearing, closest_point)
if get_distance(data.latitude, data.longitude, next_wp.latitude,
next_wp.longitude) < 0.04: # meters
waypoint_iter += 1
rospy.loginfo("Waypoint id: " + str(waypoint_iter + 2))
if waypoint_iter == 45:
# introduce angle if needed from previous file
rospy.loginfo("For distance of: " + str(distance) +
"km the integrated area is: " +
str(integrated_area) + " sq.kilometers")
exit(0)
# SETTING THE SERVICE NEW WP
# CONSTRUCTING THE WP
the_wp = Waypoint()
the_wp.frame = 3
the_wp.command = 16
the_wp.is_current = 1
the_wp.autocontinue = 1
the_wp.param1 = 0
the_wp.param2 = 10
the_wp.param3 = 0
the_wp.param4 = 0
the_wp.x_lat = c_wp.latitude
the_wp.y_long = c_wp.longitude
the_wp.z_alt = 100
the_home = Waypoint()
the_home.frame = 0
the_home.command = 22
the_home.is_current = 0
the_home.autocontinue = 1
the_home.param1 = 25
the_home.param2 = 0
the_home.param3 = 0
the_home.param4 = 0
the_home.x_lat = 41.1393967
the_home.y_long = -8.7332993
the_home.z_alt = 0
the_req = WaypointPushRequest()
the_req.waypoints.insert(0, the_home)
the_req.waypoints.insert(1, the_wp)
# SENDING THE LIST
rospy.loginfo(" Intermediate waypoint, latitude: " + str(c_wp.latitude) +
", longitude: " + str(c_wp.longitude))
command_service(the_req)
current_service(1)
def _start_mission(self):
print(self.namespace, 'waypoints')
req = WaypointPushRequest()
wp1 = Waypoint()
wp1.frame = Waypoint.FRAME_GLOBAL
wp1.command = CommandCode.NAV_TAKEOFF
wp1.is_current = True
wp1.autocontinue = True
wp1.param1 = 0.3 # minimum / desired pitch
wp1.param2 = 0.0
wp1.param3 = 0.0
wp1.param4 = 0.0 if self.color == 'blue' else -math.pi # yaw angle
wp1.x_lat = self.start_position.latitude
wp1.y_long = self.start_position.longitude
wp1.z_alt = self.start_position.altitude + ALTITUDE_ABOVE_GROUND
wp2 = copy.deepcopy(wp1)
wp2.command = CommandCode.NAV_WAYPOINT
wp2.is_current = False
wp2.param1 = 0.0
wp2.param2 = 5.0 # acceptance radius
wp2.param4 = 0.0
wp2.x_lat, wp2.y_long = cube_to_global(400.0, 250.0)
wp3 = copy.deepcopy(wp2)
wp3.autocontinue = False
wp3.x_lat, wp3.y_long = cube_to_global(100.0, 250.0)
req.waypoints = [wp1, wp2, wp3]
service_name = '%s/mavros/mission/push' % self.namespace
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, WaypointPush)
resp = service.call(req)
except rospy.ServiceException as e:
print(self.namespace,
'service call to push waypoints failed:',
str(e),
file=sys.stderr)
return False
if not resp.success:
print(self.namespace, 'failed to push waypoints', file=sys.stderr)
return False
print(self.namespace, 'pushed waypoints')
print(self.namespace, 'start mission')
req = SetModeRequest()
req.base_mode = 0 # use custom mode
# first 0x04: mode auto
# second 0x04: mission
# third and fourth 0x0000: empty
req.custom_mode = str(int('0x04040000', 0))
service_name = '%s/mavros/set_mode' % self.namespace
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, SetMode)
resp = service.call(req)
except rospy.ServiceException as e:
print(self.namespace,
'service call to set mode failed:',
str(e),
file=sys.stderr)
return False
if not resp.success:
print(self.namespace, 'failed to set mode', file=sys.stderr)
return False
print(self.namespace, 'started mission')
self._set_state(STATE_MISSION)
return True
