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 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 read_json(self, filename):
with open(filename) as json_data:
data_dict = json.load(json_data)
waypoint_list = WaypointList()
wp = Waypoint()
wp.command = CommandCode.NAV_TAKEOFF
waypoint_list.waypoints.append(wp)
wp.is_current = True
c = 0
for phase in (data_dict["phases"]):
wp = Waypoint()
wp.is_current = 0
wp.frame = 3
if phase['type'] == 'Waypoint':
wp.param1 = 0
wp.command = CommandCode.NAV_WAYPOINT
wp.autocontinue = True
wp.x_lat = phase['point']['latitude']
wp.y_long = phase['point']['longitude']
wp.z_alt = -phase['depth']
waypoint_list.waypoints.append(wp)
if phase['type'] == 'Path':
for track in phase['tracks']:
wp.param1 = 0
wp.command = CommandCode.NAV_WAYPOINT
wp.autocontinue = True
wp.x_lat = track['start_point']['latitude']
wp.y_long = track['start_point']['longitude']
wp.z_alt = track['altitude']
waypoint_list.waypoints.append(wp)
wp.command = CommandCode.NAV_SPLINE_WAYPOINT
wp.autocontinue = True
wp.x_lat = track['end_point']['latitude']
wp.y_long = track['end_point']['longitude']
wp.z_alt = track['altitude']
waypoint_list.waypoints.append(wp)
if phase['type'] == 'Dive':
wp.command = CommandCode.NAV_CONTINUE_AND_CHANGE_ALT
wp.autocontinue = True
wp.param1 = 2 # to dive and 1 to climb
wp.z_alt = -phase['depth']
waypoint_list.waypoints.append(wp)
if phase['type'] == 'Ascent':
wp.command = CommandCode.NAV_CONTINUE_AND_CHANGE_ALT
wp.autocontinue = True
wp.param1 = 1
wp.z_alt = 0
waypoint_list.waypoints.append(wp)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.waypoint_pub.publish(waypoint_list)
rate.sleep()
def setUp(self):
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state'
]
}
# ROS services
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
self.fail("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
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 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 __init__(self, node_name):
self.node_name = node_name
self.state = State()
self.mission_wp = WaypointList()
self.pos = np.array([0, 0, 0])
self.vel = np.array([0, 0, 0])
self.acc = np.array([0, 0, 0])
self.yaw = 0.
self.local_position = None
self.local_goal_point = None
self.local_goal_direction = None
self.start_pos = np.array([0, 0, 0])
self.hasLimitedVision = False
self.visionRadius = 15.
def MC_to_MR(req):
mrMission = WaypointList()
mrMission.current_seq = 0
mcMission = req.mcMission.waypoints
#set Home if given in MissionControl
if (req.mcMission.home.point.lat != 0): #Home Position is set
mrMission.waypoints.append(MR_Home_Waypoint(req.mcMission.home))
#set Gimbal Pitch from Param 'gimbalPitch'
mrMission.waypoints.append(MR_Gimbal_Pitch_Waypoint(rospy.get_param('gimbalPitch')))
counter = 0
speed = 0
cam_trigger_dist = 0
for mcWaypoint in mcMission:
#set Speed, if it has changed
if (mcWaypoint.speed != speed): #change Speed
speed = mcWaypoint.speed
mrMission.waypoints.append(MR_Speed_Waypoint(speed))
#set Nav_Waypoint
mrMission.waypoints.append(MR_Nav_Waypoint(mcWaypoint))
counter += 1
#if cam_trigger_distance would be defined in MissionControl, it could be implemented here. Same way as speed.
#until implementation, camera_trigger_distance is set after the first Waypoint
#value is set in param 'imageDistance'
if (counter == 1):
mrMission.waypoints.append(MR_Photo_Distance_Waypoint(rospy.get_param('imageDistance')))
#quit Photos
mrMission.waypoints.append(MR_Photo_Distance_Waypoint(0))
return mcTOmrResponse(mrMission)
def received_rawdata(self):
"""
input: (none)
output: thedata (std_msgs.msg.String)
returns raw data that was received,
in its original ROS format
"""
self.lock.acquire()
try:
thedata = self.mystring_ros
self.mystring_ros = WaypointList()
finally:
self.lock.release()
return thedata
def setMission(positions, timeout=WAIT_FOR_MAVLINK_TIMEOUT):
"""Upload mission of passed positions.
"""
wl = WaypointList()
pos = positions[0]
wp = Waypoint()
wp.command = MAV_CMD_NAV_TAKEOFF
wp.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT
wp.is_current = True
wp.autocontinue = True
wp.x_lat = pos.lat
wp.y_long = pos.lon
wp.z_alt = pos.alt
wl.waypoints.append(wp)
for pos in positions[1:]:
wp = Waypoint()
wp.command = MAV_CMD_NAV_WAYPOINT
wp.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT
wp.is_current = False
wp.autocontinue = True
wp.x_lat = pos.lat
wp.y_long = pos.lon
wp.z_alt = pos.alt
wl.waypoints.append(wp)
wp = Waypoint()
wp.command = MAV_CMD_NAV_LAND
wp.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT
wp.is_current = False
wp.autocontinue = True
wp.x_lat = pos.lat
wp.y_long = pos.lon
wp.z_alt = 0
wl.waypoints.append(wp)
try:
wpPushService.wait_for_service(timeout=timeout)
ret = wpPushService(0, wl.waypoints)
except rospy.ROSException, err:
Info("Waypoint push call failed with error '%s'", err)
return None
def setUp(self):
super(MavrosOffboardPosctlTest, self).setUp()
self.matlab_topics_ready = {key: False for key in ['task']}
self.pos = PoseStamped()
self.radius = 1
# Subscribing to matlab message
self.matlab_task = WaypointList()
self.matlab_task_sub = rospy.Subscriber("/task", PoseStamped,
self.matlab_callback)
self.pos_setpoint_pub = rospy.Publisher(
'/uav0/mavros/setpoint_position/local', PoseStamped, queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
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 __init__(self):
self.br = None
self.server = None
self.rate_config = rospy.get_param('~rate', 10.0)
self.current_waypoint_position_x = rospy.get_param(
'~current_waypoint_position_x', 0.0)
self.current_waypoint_position_y = rospy.get_param(
'~current_waypoint_position_y', 0.0)
self.current_waypoint_position_z = rospy.get_param(
'~current_waypoint_position_z', 0.0)
self.g_set_state = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
# mavros marker setup
# configure waypoint publisher
self.mavros_waypoint_pub = rospy.Publisher("/mavros/mission/waypoints",
Waypoint,
queue_size=1)
self.waypoint_msg = WaypointList()
# define rate
self.rate = rospy.Rate(self.rate_config)
self.br = TransformBroadcaster()
self.server = InteractiveMarkerServer("waypoint_interactive_publisher")
position = Point(self.current_waypoint_position_x,
self.current_waypoint_position_y, 0)
self.make_auv_waypoint_Marker(position)
self.server.applyChanges()
# infinity loop
while not rospy.is_shutdown():
self.update_waypoints()
self.rate.sleep()
#!/usr/bin/env python
import rospy
import os
import json
from sensor_msgs.msg import Imu
from mavros import mavlink
from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, State, WaypointList
from mavros_msgs.srv import WaypointPush, SetMode, CommandBool
from pymavlink import mavutil
from threading import Thread
state = State()
mission_wp = WaypointList()
# Execute the mission that has been specified.
def mission(mission_file_path):
try:
mission = read_mission(mission_file_path)
except IOError as e:
print e
send_wps(mission, 30)
def send_wps(waypoints, timeout):
global mission_wp
rospy.loginfo("sending mission waypoints")
loop_freq = 1 # Hz
rate = rospy.Rate(loop_freq)
def __init__(self, *args):
super(MavrosQuad, self).__init__(*args)
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.local_velocity = TwistStamped()
self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link'
self.gazebo_load_pose = Pose()
self.gazebo_load_twist = Twist()
self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0'
self.gazebo_quad_pose = Pose()
self.gazebo_quad_twist = Twist()
self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link'
self.gazebo_imu_pose = Pose()
self.gazebo_imu_twist = Twist()
self.q_input = np.array([0, 0, 0, 1])
self.T_input = 0
self.offboard_position_active = True
self.offboard_attitude_active = False
self.offboard_load_active = False
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos',
'local_pos', 'local_vel', 'mission_wp', 'state', 'imu'
]
}
# ROS services
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
rospy.logerr("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.local_vel_sub = rospy.Subscriber(
'mavros/local_position/velocity_local', TwistStamped,
self.local_velocity_callback)
self.imu_sub = rospy.Subscriber('/mavros/imu/data', Imu,
self.imu_callback)
self.gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates,
self.gazebo_callback)
# ROS Publishers
# Attitude
self.att = AttitudeTarget()
self.att_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.att_thread = Thread(target=self.send_att, args=())
self.att_thread.daemon = True
self.att_thread.start()
# Pose
self.pos = PoseStamped()
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
def __init__(self,
ros_rate=10, # slow as nothing happens in the main loop
state_estimation_mode=State_estimation_method.GPS,
enforce_height_mode_flag=False,
height_mode_req=0,
):
self.sem = state_estimation_mode
self._node_alive = True
self.ros_rate = ros_rate
self.wd_initialised = False
self.wd_fault_detected = False
self.fault_this_loop = False
# initialise data containers
self.altitude = Altitude()
self.altitude_bottom_clearance = Float32()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.optic_flow_raw = OpticalFlowRad()
self.optic_flow_range = Range()
self.home_position = HomePosition()
self.local_position = PoseStamped()
# self.gt_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mocap_pose = PoseStamped()
self.camera_pose = PoseStamped()
self.camera_yaw = None
self.local_x = Float64().data
self.local_y = Float64().data
self.local_z = Float64().data
self.gt_x = Float64().data
self.gt_y = Float64().data
self.gt_z = Float64().data
# todo - we can probably live with just XX_vel_bod data?:
self.x_vel = Float64().data
self.y_vel = Float64().data
self.gt_x_vel = Float64().data
self.gt_y_vel = Float64().data
self.vel_ts = Float64().data
self.xy_vel = Float64().data
self.x_vel_bod = Float64().data
self.y_vel_bod = Float64().data
self.xy_vel_bod = Float64().data
self.body_yaw_rate = Float64().data
self.global_compass_hdg_deg = Float64().data
self.enforce_height_mode_flag = enforce_height_mode_flag
self.height_mode_req = height_mode_req
# threading locks
self.setpoint_lock = Lock() # used for setting lock in our setpoint publisher so that commands aren't mixed
# ROS services
service_timeout = 10
rospy.loginfo("Searching for mavros services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/param/set', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.wait_for_service('/mavros/cmd/set_home')
# rospy.wait_for_service('mavros/fcu_url', service_timeout) # todo - check how this is used in px4
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_param_srv = rospy.ServiceProxy('mavros/param/set', ParamSet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush)
self.takeoff_srv = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)
self.cmd_home_srv = rospy.ServiceProxy('/mavros/cmd/set_home', CommandHome)
rospy.loginfo("Required ROS services are up")
except rospy.ROSException:
self.shut_node_down(extended_msg="failed to connect to Mavros services - was the mavros node started?")
# make sure we have information about our connection with FCU
rospy.loginfo("Get our fcu string")
try:
self.fcu_url = rospy.get_param('mavros/fcu_url')
except Exception as e:
print (e)
self.shut_node_down(extended_msg="cant find fcu url")
# ensure that our height mode is as we expect it to be (if required)
rospy.loginfo('check height_mode {}'.format(self.enforce_height_mode_flag))
if self.enforce_height_mode_flag:
# todo - allow multiple attempts at this
# res = self.mavros_interface.get_param_srv(param)
param_read_attempts = 0
try:
while param_read_attempts < 5:
res = self.get_param_srv('EKF2_HGT_MODE')
if res.success:
self.height_mode = res.value.integer
if self.height_mode == self.height_mode_req:
rospy.loginfo('height mode {} as expected'.format(self.height_mode))
break
else:
raise Exception ("height mode is {} - (expected heightmode is {}) change parameter with QGround control and try again".format(self.height_mode, self.height_mode_req))
break
else:
rospy.logerr( "Couldn't read EKF2_HGT_MODE param on attempt {} - trying again".format(param_read_attempts))
param_read_attempts += 1
rospy.sleep(2)
except Exception as e:
rospy.logerr(
"Couldn't read EKF2_HGT_MODE - shutting down".format(param_read_attempts))
self.shut_node_down(extended_msg= "height_mode error - traceback is {}".format(e))
# todo: ensure that our state estimation parameters are as available (this requires the state estimation
# topic name so can't test this until we do something with mocap again) Actually, we can incorporate this into
# the watchdog
# if state_estimation_mode == State_estimation_method.MOCAP:
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',ExtendedState,self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',NavSatFix, self.global_position_callback)
self.optic_flow_raw_sub = rospy.Subscriber('mavros/px4flow/raw/optical_flow_raw',OpticalFlowRad, self.optic_flow_raw_callback)
self.optic_flow_range_sub = rospy.Subscriber('mavros/px4flow/ground_distance',Range,self.optic_flow_range_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback)
self.mocap_pos_sub = rospy.Subscriber('mavros/vision_pose/pose', PoseStamped, self.mocap_pos_callback)
# self.camera_pose_sub = rospy.Subscriber(self.camera_pose_topic_name, PoseStamped, self.cam_pose_cb)
# todo - add check for this signal to watchdog - or remap /mavros/local_position/velocity -> /mavros/local_position/velocity_local
self.velocity_local_sub = rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.vel_callback)
self.velocity_body_sub = rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.vel_bod_callback)
self.compass_sub = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, self.compass_hdg_callback)
self.ground_truth_sub = rospy.Subscriber('/body_ground_truth', Odometry, self.gt_position_callback)
## Ros publishers
self.local_pos_pub_raw = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
# ROS topics - this must come after our ROS subscribers
topics_timeout = 30
rospy.loginfo("waiting for ROS topics")
try:
# check that essential messages are being subscribed to regularly
for _ in np.arange(2):
rospy.wait_for_message('mavros/local_position/pose', PoseStamped, topics_timeout)
rospy.wait_for_message('mavros/extended_state', ExtendedState, topics_timeout)
except rospy.ROSException:
self.shut_node_down(extended_msg="Required ros topics not published")
rospy.loginfo("ROS topics are up")
# create a watchdog thread that checks topics are being received at the expected rates
self.watchdog_thread = Thread(target=self.watchdog, args=())
self.watchdog_thread.daemon = True
self.watchdog_thread.start()
def __init__(self):
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.imu_data = Imu()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.pos = PoseStamped()
self.radius = 1
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state', 'imu'
]
}
# ROS services
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
self.fail("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.imu_data_sub = rospy.Subscriber('mavros/imu/data', Imu,
self.imu_data_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
# ROS publisher
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=1)
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
def __init__(self, *args):
super(MavrosQuad, self).__init__(*args)
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.local_velocity = TwistStamped()
self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link'
self.gazebo_load_pose = Pose()
self.gazebo_load_twist = Twist()
self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0'
self.gazebo_quad_pose = Pose()
self.gazebo_quad_twist = Twist()
self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link'
self.gazebo_imu_pose = Pose()
self.gazebo_imu_twist = Twist()
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos',
'local_pos', 'local_vel', 'mission_wp', 'state', 'imu'
]
}
# ROS services
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
rospy.logerr("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state',
State,
self.state_callback)
self.local_vel_sub = rospy.Subscriber('mavros/local_position/velocity_local',
TwistStamped,
self.local_velocity_callback)
self.imu_sub = rospy.Subscriber('/mavros/imu/data',
Imu,
self.imu_callback)
self.gazebo_sub = rospy.Subscriber('/gazebo/link_states',
LinkStates,
self.gazebo_callback)
def setUp(self):
print('@ctrl_server@ uav{0} setUp'.format(self.uav_number))
self.pos = PoseStamped()
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.misson_wp = WaypointList()
self.state = State()
self.scan = LaserScan()
self.mav_type = None
self.ready = False
# Target offset radius
self.radius = 0.25
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state', 'scan'
]
}
# ROS services
service_timeout = 60
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service(self.uav_prefix + 'mavros/param/get', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/cmd/arming', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/mission/push', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/mission/clear', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
print("@ctrl_server@ failed to connect to services")
self.get_param_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/param/get',
ParamGet)
self.set_arming_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/set_mode',
SetMode)
self.wp_clear_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber(self.uav_prefix + 'mavros/altitude',
Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber(self.uav_prefix + 'mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber(self.uav_prefix + 'mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber(self.uav_prefix + 'mavros/state', State,
self.state_callback)
self.get_scan_srv = rospy.Subscriber('iris_rplidar_' + self.uav_number + '/laser/scan',
LaserScan,
self.scan_callback)
# ROS publisers
self.pos_setpoint_pub = rospy.Publisher(
self.uav_prefix + 'mavros/setpoint_position/local', PoseStamped, queue_size=1)
# send setpoints in seperate thread
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
print('@ctrl_server@ setUp over')
pass
def task_listener(self):
self.matlab_task = WaypointList()
rospy.init_node('/uav0_task')
self.matlab_task_sub = rospy.Subscriber("task", WaypointList,
self.matlab_callback)
rospy.spin()
def __init__(self):
self.mystring = None #"" # unpacked
self.mystring_ros = WaypointList() # raw format
self.lock = threading.Lock()
def handle_set_mission_waypoints(req):
global lat_x
global lng_y
global alt
lat_x = req.lat
lng_y = req.lng
alt = req.height_agl
print "MISSION WAYPOINTS REQUEST -> "
print req
print ""
if lat_x is None:
message = "No waypoints received."
return False, message
else:
publish_waypoint_data(lat_x, lng_y)
waypoint_list = []
final_waypoint = WaypointList()
# for i in range(len(lat_x)):
# # print "reached "
# # x = lat_x[i]
# # y = lng_y[i]
# # z = alt[i]
# print "X AND Y COORDS ->"
# print x
# print y
# print "--------------"
# try:
# print "Waiting for position_set_global"
# rospy.wait_for_service('/flytos/navigation/position_set_global',timeout=2)
# print "After "
# handle = rospy.ServiceProxy('/flytos/navigation/position_set_global', PositionSetGlobal)
# req_msg = PositionSetGlobalRequest(lat_x=x, long_y=y, rel_alt_z=z, yaw=0, tolerance=0.2, async=False, yaw_valid=False)
# resp = handle(req_msg)
# except rospy.ServiceException, e:
# rospy.logerr("global pos set service call failed %s", e)
# return False, e
for i in range(len(lat_x)):
waypoint = Waypoint()
waypoint.frame = 3
waypoint.command = 16
if i == 0:
waypoint.is_current = True
else:
waypoint.is_current = False
waypoint.autocontinue = True
waypoint.param1 = 2.0
waypoint.param2 = 0
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat_x[i]
waypoint.y_long = lng_y[i]
waypoint.z_alt = 2.5
print "Printing Waypoint ....."
print waypoint
waypoint_list.append(waypoint)
final_waypoint.waypoints = [waypoint_list]
try:
rospy.wait_for_service('/flytos/navigation/waypoint_set',
timeout=10)
handle = rospy.ServiceProxy('/flytos/navigation/waypoint_set',
WaypointSet)
req_msg = WaypointSetRequest(waypoints=waypoint_list)
resp = handle(req_msg)
message = "Sent mission waypoints to the drone successfully."
if resp.success is True:
print "Mission sent successfully to the drone.Executing setpoints."
try:
rospy.wait_for_service(
'/flytos/navigation/waypoint_execute')
handle = rospy.ServiceProxy(
'/flytos/navigation/waypoint_execute', WaypointExecute)
req_msg = WaypointExecuteRequest()
resp = handle(req_msg)
message = "Executing mission!"
return resp.success, resp.message
except Exception as e:
print e
return False, str(e)
except Exception as e:
message = "Error sending waypoints to the drone" + " " + str(e)
return False, message
