def setUp(self):
rospy.init_node('test_node', anonymous=True)
self.rate = rospy.Rate(10) # 10hz
self.has_global_pos = False
self.local_position = PoseStamped()
self.global_position = NavSatFix()
self.extended_state = ExtendedState()
self.home_alt = 0
self.mc_rad = 5
self.fw_rad = 60
self.fw_alt_rad = 10
self.last_alt_d = 9999
self.last_pos_d = 9999
self.mission_name = ""
# need to simulate heartbeat for datalink loss detection
rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
rospy.wait_for_service('mavros/cmd/command', 30)
self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command',
CommandLong,
persistent=True)
self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push',
WaypointPush,
persistent=True)
rospy.Subscriber("mavros/local_position/pose", PoseStamped,
self.position_callback)
rospy.Subscriber("mavros/global_position/global", NavSatFix,
self.global_position_callback)
rospy.Subscriber("mavros/extended_state", ExtendedState,
self.extended_state_callback)
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 _are_agents_on_ground(self):
topics = [t for t in self.topics if 'mavros/extended_state' in t]
for topic in topics:
extended = ExtendedState()
extended = rospy.wait_for_message(topic, ExtendedState)
drone = get_drone(topic)
if extended.landed_state != extended.LANDED_STATE_ON_GROUND:
rospy.logerr(
'{} disarm failed. Vehicle not on ground.'.format(drone))
return False
return True
def setUp(self):
self.rate = rospy.Rate(10) # 10hz
self.has_global_pos = False
self.global_position = NavSatFix()
self.extended_state = ExtendedState()
self.altitude = Altitude()
self.state = State()
self.mc_rad = 5
self.fw_rad = 60
self.fw_alt_rad = 10
self.last_alt_d = None
self.last_pos_d = None
self.mission_name = ""
self.sub_topics_ready = {
key: False
for key in ['global_pos', 'home_pos', 'ext_state', 'alt', 'state']
}
# setup ROS topics and services
try:
rospy.wait_for_service('mavros/mission/push', 30)
rospy.wait_for_service('mavros/cmd/arming', 30)
rospy.wait_for_service('mavros/set_mode', 30)
except rospy.ROSException:
self.fail("failed to connect to mavros services")
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
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.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
# need to simulate heartbeat to prevent datalink loss detection
self.hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(
mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0)
self.hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1))
self.hb_ros_msg = mavlink.convert_to_rosmsg(self.hb_mav_msg)
self.hb_thread = Thread(target=self.send_heartbeat, args=())
self.hb_thread.daemon = True
self.hb_thread.start()
def __init__(self,debug,rate):
rospy.Subscriber('/mavros/state', State, self.stateCb)
rospy.Subscriber('/mavros/extended_state', ExtendedState, self.extstateCb)
rospy.Subscriber('/mavros/rc/in', RCIn, self.rcin_update)
self.modeHandle = rospy.ServiceProxy("/mavros/set_mode", SetMode)
self.armHandle= rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.landHandle= rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
self.currentState = State()
self.extState = ExtendedState()
self.rate = rate
self.debug = debug
self.rcResumeSw = False
def __init__(self, controller_name='mpc'):
# super(offboard_controller, self).__init__() # only workable for python3
# mavros_core_function.__init__(self, ) # handle default mavros info
self.last_request = None
self.state = State()
self.extended_state = ExtendedState()
## Topics
self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
rospy.Subscriber('/mavros/state', State, self.mavros_state_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
## Service
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
## Timer
self.rate = rospy.Rate(50)
if controller_name == 'pid':
rospy.wait_for_service('uav_pid_server')
self.control_state_check_function = rospy.ServiceProxy(
'uav_pid_server', SetBool)
self.controller_state = self.control_state_check_function(True)
# print(self.controller_state)
elif controller_name == 'mpc':
rospy.wait_for_service('uav_mpc_server')
self.control_state_check_function = rospy.ServiceProxy(
'uav_mpc_server', SetBool)
self.controller_state = self.control_state_check_function(True)
else:
rospy.info("Unknown controller")
# self.set_local_position()
if self.controller_state.success:
# setup the arm and ready for the flight
# self.set_arm(True, 10)
self.offboard_ready = True
else:
rospy.loginfo(
'UAV controller server is not ready yet, please launch a controller first'
)
self.offboard_ready = False
def __init__(self):
self.state = State()
self.altitude = Altitude()
self.global_position = HomePosition()
self.local_position = PoseStamped() #fcu local position
self.local_position_odom = Odometry()
self.extended_state = ExtendedState()
self.mode = ''
self.rate = rospy.Rate(10)
self.service_timeout = 30
self.offboard_request_threshold = rospy.Duration(5.0)
self.arming_request_threshold = rospy.Duration(5.0)
self.imu = Imu()
self.current_yaw = None
self.setup_pubsub()
self.setup_services()
self.current_target_pose = PositionTarget()
self.current_pose = Point()
self.home_pose = Point(0.0,0.0,0.0)
self.waypoint_1 = Point(0.0,0.0,3.0)
self.should_start_mission = False
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):
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.local_position = PoseStamped()
self.state = State()
self.mav_type = None
self.local_velocity = TwistStamped()
self.sub_topics_ready = {
key: False
for key in
['alt', 'ext_state', 'global_pos', 'local_pos', '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)
# Subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.local_vel_sub = rospy.Subscriber(
'mavros/local_position/velocity_body', TwistStamped,
self.local_vel_callback)
# Publishers
self.desired_vel = TwistStamped()
self.desired_vel.twist.angular.z = 0
self.desired_vel.twist.linear.x = 0
self.desired_vel.twist.linear.y = 0
self.desired_vel.twist.linear.z = 0
#self.radius = 0.3
#self.yaw_th = 0.05
self.yaw_current = 0
self.isLand = False
self.yawrate = 0.5
self.xvel = 0.5
self.z_target = 0
self.z_in = 0.1
self.z_ub = 2.5 # Height upper bound
self.z_lb = 0.5 # Height lower bound
self.z_p_gain = 1.0
self.cmd_vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_vel, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
# Keyboard input thread
self.key_thread = Thread(target=self.keyInput, args=())
self.key_thread.daemon = True
self.key_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.local_position = PoseStamped()
self.state = State()
self.mav_type = None
self.prev_position_x = 0
self.prev_position_y = 0
self.prev_position_z = 0
self.drift_x = 0
self.drift_y = 0
self.drift_z = 0
self.drift_threshold = 0.07
self.IsDrift = False
self.sub_topics_ready = {
key: False
for key in
['alt', 'ext_state', 'global_pos', 'local_pos', '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.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
# Publisher
self.desired_pos = PoseStamped()
self.desired_pos.pose.position.x = 0
self.desired_pos.pose.position.y = 0
self.desired_pos.pose.position.z = 0
self.radius = 0.3
self.yaw_th = 0.05
self.z_in = 0.1
self.z_ub = 2.7 # Height upper bound
self.z_lb = 0.5 # Height lower bound
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, context):
super(PosTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PosTeleop')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'PositionTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('PosTeleopUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add logo
pixmap = QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.play_code_flag = False
self.takeoff = False
self._widget.term_out.setReadOnly(True)
self._widget.term_out.setLineWrapMode(self._widget.term_out.NoWrap)
# Set functions for each GUI Item
self._widget.takeoffButton.clicked.connect(self.call_takeoff_land)
self._widget.stopButton.clicked.connect(self.stop_drone)
self._widget.playButton.clicked.connect(self.call_play)
self._widget.posControlButton.clicked.connect(self.set_pos)
# Add Publishers
self.play_stop_pub = rospy.Publisher('gui/play_stop',
Bool,
queue_size=1)
# Add global variables
self.extended_state = ExtendedState()
self.shared_pose_msg = Pose()
self.shared_twist_msg = Twist()
self.current_pose = Pose()
self.pose_frame = ''
self.current_twist = Twist()
self.twist_frame = ''
self.is_running = True
self.stop_icon = QIcon()
self.stop_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.sensors_widget = SensorsWidget(self)
self._widget.sensorsCheck.stateChanged.connect(
self.show_sensors_widget)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscribers
rospy.Subscriber('drone_wrapper/local_position/pose', PoseStamped,
self.pose_stamped_cb)
rospy.Subscriber('drone_wrapper/local_position/velocity_body',
TwistStamped, self.twist_stamped_cb)
rospy.Subscriber('drone_wrapper/extended_state', ExtendedState,
self.extended_state_cb)
# Add Timer
self.update_status_info()
def __init__(self, screen):
rospy.init_node('status_node', argv=sys.argv)
self.rate = rospy.get_param('~rate', default=1.0)
# Curses setup
self.screen = curses.initscr()
self.rows, self.cols = self.screen.getmaxyx()
height_status = 15
self.status = curses.newwin(height_status, self.cols, 1, 2)
# self.console = curses.newwin(self.rows - height_status, self.cols, 12, 2)
self.lines = 0
self.text = ''
self.screen.keypad(True)
curses.curs_set(False) # Hide cursor
colors = [
curses.COLOR_BLACK, curses.COLOR_BLUE, curses.COLOR_CYAN,
curses.COLOR_GREEN, curses.COLOR_MAGENTA, curses.COLOR_RED,
curses.COLOR_WHITE, curses.COLOR_YELLOW
]
# Curses color setup
curses.use_default_colors()
for color in colors:
curses.init_pair(color, color, -1)
# Default variables
self.status_battery_perc = None
self.state = State()
self.state_sub = rospy.Subscriber('mavros/state',
State,
callback=self.state_callback,
queue_size=1)
self.battery = BatteryState()
self.battery_sub = rospy.Subscriber('mavros/battery',
BatteryState,
callback=self.battery_callback,
queue_size=1)
self.extended = ExtendedState()
self.extended_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
callback=self.extended_callback,
queue_size=1)
# self.statustext = StatusText()
# self.statustext_sub = rospy.Subscriber('mavros/statustext/recv', StatusText,
# callback=self.statustext_callback,
# queue_size=1)
self.gps = NavSatFix()
self.gps_sub = rospy.Subscriber('mavros/global_position/raw/fix',
NavSatFix,
callback=self.gps_callback,
queue_size=1)
self.local_pose = PoseStamped()
self.local_pose_sub = rospy.Subscriber(
'mavros/local_position/pose',
PoseStamped,
callback=self.local_pose_callback,
queue_size=1)
self.global_pose = PoseStamped()
self.global_pose_sub = rospy.Subscriber(
'global_position/pose',
PoseStamped,
callback=self.global_pose_callback,
queue_size=1)
self.diagnostics = DiagnosticArray()
self.diagnostic_gps = DiagnosticStatus()
self.diagnostics_sub = rospy.Subscriber(
'/diagnostics',
DiagnosticArray,
callback=self.diagnostics_callback,
queue_size=1)
self.setpoint = PositionTarget()
self.setpoint_sub = rospy.Subscriber('mavros/setpoint_raw/local',
PositionTarget,
callback=self.setpoint_callback,
queue_size=1)
self.cameras = ['front', 'right', 'back', 'left']
self.image_subscribers = []
self.images = {c: deque(maxlen=10) for c in self.cameras}
for camera in self.cameras:
topic = f'camera_{camera}/image_raw'
subscriber = rospy.Subscriber(topic,
Image,
callback=self.image_callback,
callback_args=camera,
queue_size=1,
buff_size=2**24)
self.image_subscribers.append(subscriber)
def __init__(self, name='drone', ns='', verbose=False):
if name != 'rqt':
if verbose:
rospy.init_node(name, anonymous=True, log_level=rospy.DEBUG)
else:
rospy.init_node(name)
self.ns = ns
self.state = State()
self.extended_state = ExtendedState()
self.pose_stamped = PoseStamped()
self.vel_body_stamped = TwistStamped()
self.rate = rospy.Rate(20)
self.setpoint_raw = PositionTarget()
self.setpoint_raw_flag = False
self.vz_factor = 0.4
self.bridge = CvBridge()
self.is_z = False
self.is_xy = False
self.posx = 0
self.posy = 0
self.height = 0
self.vx = 0
self.vy = 0
self.vz = 0
self.setpoint_raw_timer = rospy.Timer(rospy.Duration(nsecs=50000000),
self.repeat_setpoint_raw)
self.setpoint_raw_timer.shutdown()
self.stay_armed_stay_offboard_timer = rospy.Timer(
rospy.Duration(5), self.stay_armed_stay_offboard_cb)
self.stay_armed_stay_offboard_timer.shutdown()
rospy.wait_for_service(self.ns + 'mavros/cmd/arming')
self.arm_client = rospy.ServiceProxy(ns + 'mavros/cmd/arming',
CommandBool)
rospy.wait_for_service(self.ns + 'mavros/set_mode')
self.mode_client = rospy.ServiceProxy(ns + 'mavros/set_mode', SetMode)
rospy.wait_for_service(self.ns + 'mavros/cmd/land')
self.land_client = rospy.ServiceProxy(ns + 'mavros/cmd/land',
CommandTOL)
self.rqt_extended_state_publisher = rospy.Publisher(
self.ns + 'drone_wrapper/extended_state',
ExtendedState,
queue_size=1)
self.rqt_pose_publisher = rospy.Publisher(
self.ns + 'drone_wrapper/local_position/pose',
PoseStamped,
queue_size=1)
self.rqt_velocity_body_publisher = rospy.Publisher(
self.ns + 'drone_wrapper/local_position/velocity_body',
TwistStamped,
queue_size=1)
self.rqt_cam_frontal_publisher = rospy.Publisher(
self.ns + 'drone_wrapper/cam_frontal/image_raw',
Image,
queue_size=1)
self.rqt_cam_ventral_publisher = rospy.Publisher(
self.ns + 'drone_wrapper/cam_ventral/image_raw',
Image,
queue_size=1)
rospy.Subscriber(self.ns + 'mavros/state', State, self.state_cb)
rospy.Subscriber(self.ns + 'mavros/extended_state', ExtendedState,
self.extended_state_cb)
rospy.Subscriber(self.ns + 'mavros/local_position/pose', PoseStamped,
self.pose_stamped_cb)
rospy.Subscriber(self.ns + 'mavros/local_position/velocity_body',
TwistStamped, self.vel_body_stamped_cb)
rospy.Subscriber(self.ns + 'mavros/global_position/global', NavSatFix,
self.global_position_cb)
cam_frontal_topic = rospy.get_param('cam_frontal_topic',
'/iris/cam_frontal/image_raw')
cam_ventral_topic = rospy.get_param('cam_ventral_topic',
'/iris/cam_ventral/image_raw')
rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb)
rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb)
self.setpoint_raw_publisher = rospy.Publisher(
self.ns + 'mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
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 __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()
class OffbPosCtl:
curr_pose = PoseStamped()
des_pose = PoseStamped()
init_pose = PoseStamped()
curr_state = ExtendedState()
def __init__(self):
rospy.init_node('offboard_sequences', anonymous=True)
self.pose_pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
self.pose_sub = rospy.Subscriber('/mavros/local_position/pose',
PoseStamped,
callback=self.pose_cb)
self.ext_state_sub = rospy.Subscriber('/mavros/extended_state',
ExtendedState,
callback=self.ext_state_cb)
self.rate = rospy.Rate(10) # Hz
self.rate.sleep()
self.init_pose = self.copy_pose(self.curr_pose)
self.des_pose = self.copy_pose(self.curr_pose)
print self.init_pose
def takeoff(self):
"""
UAV will takeoff to a height defined as TAKEOFF_HEIGHT, maintaining the initial x,y coordinates and orientation.
"""
if not rospy.is_shutdown():
print "offboard takeoff initiated"
init_time = rospy.get_time()
while rospy.Duration.from_sec(rospy.get_time() - init_time
) < rospy.Duration(HOVER_DURATION):
if self.curr_pose.pose.position.z < TAKEOFF_HEIGHT and self.des_pose.pose.position.z < TAKEOFF_HEIGHT:
self.des_pose.pose.position.z += MOTION_INCREMENT
else:
self.des_pose.pose.position.z = TAKEOFF_HEIGHT
self.pose_pub.publish(self.des_pose)
# print self.des_pose
self.rate.sleep()
def land(self):
"""
UAV will land at the current x,y position, maintaining orientation.
"""
print "offboard landing initiated"
init_z = self.init_pose.pose.position.z
landed = False
while not rospy.is_shutdown():
while not landed:
if self.curr_state.landed_state != 1:
self.des_pose.pose.position.z -= MOTION_INCREMENT
self.pose_pub.publish(self.des_pose)
# print self.des_pose
self.rate.sleep()
else:
landed = True
landed_pose = self.copy_pose(self.curr_pose)
# print "landed: " + str(landed)
self.pose_pub.publish(
landed_pose) # TODO quad moves after landing. change mode?
# print landed_pose
self.rate.sleep()
def circle(self):
"""
UAV will complete a circular path, around (0,0) at the current height and orientation.
"""
print "circle path initiated"
t_init = rospy.get_time()
t = rospy.Duration(0)
while t <= rospy.Duration(7):
self.des_pose.pose.position.x = CIRCLE_RAD * 1
self.des_pose.pose.position.y = CIRCLE_RAD * 0
self.pose_pub.publish(self.des_pose)
# print self.des_pose
self.rate.sleep()
t = rospy.Duration.from_sec(rospy.get_time() - t_init)
t_init = rospy.get_time()
t = 0
while t <= NCIRCLES:
theta = t * 2 * math.pi
self.des_pose.pose.position.x = CIRCLE_RAD * math.cos(theta)
self.des_pose.pose.position.y = CIRCLE_RAD * math.sin(theta)
self.pose_pub.publish(self.des_pose)
# print self.des_pose
self.rate.sleep()
t = (rospy.get_time() - t_init) / CIRCLE_DURATION
def square(self):
"""
UAV will complete a square path, around (0,0) at the current height and orientation.
"""
print "square path initiated"
SIDE = 2
HALF_SIDE = SIDE / 2
corners = [(HALF_SIDE, HALF_SIDE), (-HALF_SIDE, HALF_SIDE),
(-HALF_SIDE, -HALF_SIDE), (HALF_SIDE, -HALF_SIDE)]
for corner in corners:
t_init = rospy.get_time()
t = rospy.Duration(0)
while t <= rospy.Duration(8):
self.des_pose.pose.position.x = corner[0]
self.des_pose.pose.position.y = corner[1]
self.pose_pub.publish(self.des_pose)
# print self.des_pose
self.rate.sleep()
t = rospy.Duration.from_sec(rospy.get_time() - t_init)
def copy_pose(self, pose):
"""
creates a copy of a PoseStamped object
"""
pt = pose.pose.position
quat = pose.pose.orientation
copied_pose = PoseStamped()
copied_pose.header.frame_id = pose.header.frame_id
copied_pose.header.stamp = rospy.Time.now()
copied_pose.pose.position = Point(pt.x, pt.y, pt.z)
copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z,
quat.w)
return copied_pose
def pose_cb(self, msg):
# print msg
self.curr_pose = msg
def ext_state_cb(self, msg):
self.curr_state = msg
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