def __init__(self):
rospy.init_node("mav_control_node")
rospy.Subscriber("/mavros/global_position/global",NavSatFix,self.gps_callback)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback)
rospy.Subscriber('mavros/state',State,self.arm_callback)
rospy.Subscriber('mavros/battery',BatteryState,self.battery_callback)
rospy.Subscriber('mavros/global_position/raw/satellites',UInt32,self.sat_num_callback)
rospy.Subscriber('mavros/global_position/rel_alt',Float64,self.rel_alt_callback)
rospy.Subscriber('mavros/imu/data',Imu,self.imu_callback)
rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback)
rospy.Subscriber('mavros/vfr_hud',VFR_HUD,self.hud_callback)
self.gps = NavSatFix()
self.pose = PoseStamped()
self.arm_status = State()
self.battery_status = BatteryState()
self.sat_num = UInt32()
self.rel_alt = Float64()
self.imu = Imu()
self.rc = RCIn()
self.hud = VFR_HUD()
self.timestamp = rospy.Time()
self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1)
self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1)
self.mode_service = rospy.ServiceProxy("/mavros/set_mode", SetMode)
self.arm_service = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
self.takeoff_service = rospy.ServiceProxy("/mavros/cmd/takeoff", CommandTOL)
self.stream_service = rospy.ServiceProxy("/mavros/set_stream_rate", StreamRate)
self.home_service = rospy.ServiceProxy("/mavros/cmd/set_home", CommandHome)
def __init__(self):
rospy.init_node("mav_control_node")
rospy.Subscriber("/mavros/local_position/pose", PoseStamped,
self.pose_callback)
rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback)
self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=1)
self.cmd_vel_pub = rospy.Publisher(
"/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1)
self.rc_override = rospy.Publisher("/mavros/rc/override",
OverrideRCIn,
queue_size=1)
# mode 0 = STABILIZE
# mode 4 = GUIDED
# mode 9 = LAND
self.mode_service = rospy.ServiceProxy('/mavros/set_mode', SetMode)
self.arm_service = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.takeoff_service = rospy.ServiceProxy('/mavros/cmd/takeoff',
CommandTOL)
self.rc = RCIn()
self.pose = Pose()
self.timestamp = rospy.Time()
def rc_publisher(pub):
# publish rc signal
rc_data = RCIn()
rc_input = np.zeros(12)
rate = rospy.Rate(3000)
while not rospy.is_shutdown():
rc_input[0:5] = rc.rc(5)
t = math.modf(time.time())
rc_data.header.stamp.secs = int(t[1])
rc_data.header.stamp.nsecs = int(t[0] * 10**9)
rc_data.channels = rc_input
rc_data.rssi = 128
rospy.loginfo(rc_data)
pub.publish(rc_data)
rate.sleep()
def __init__(self, **kwargs):
# Initial FSM state
self.state = VehicleState.ERROR
self.statedesc = 'ERROR'
self.statename = 'Initializing'
# ROS rate (1 Hz)
self.rate = rospy.Rate(1)
# Parameters
self.vehicle_name = socket.gethostname()
self.fix_topic = rospy.get_param(
"asv_description/fix_topic",
default='/mavros/global_position/raw/fix')
self.rec_topic = rospy.get_param(
"asv_description/record_command_topic", default='/mavros/rc/in')
self.rec_cmd_channel = rospy.get_param(
"asv_description/record_command_channel", default=5) - 1
self.rec_cmd_threshold = rospy.get_param(
"asv_description/record_command_threshold", default=20)
# Publishers and subscribers
self.status_pub = rospy.Publisher('/%s/vehicle/state' %
socket.gethostname(),
VehicleState,
queue_size=10)
self.diag_agg_sub = rospy.Subscriber('/diagnostics_agg',
DiagnosticArray,
callback=self.diag_callback)
self.diag_agg_sub = rospy.Subscriber(
'/diagnostics_toplevel_state',
DiagnosticStatus,
callback=self.diag_toplevel_callback)
self.rec_cmd_sub = rospy.Subscriber(self.rec_topic,
RCIn,
callback=self.rec_callback)
self.fix_sub = rospy.Subscriber(self.fix_topic,
NavSatFix,
callback=self.fix_callback)
# Messages
self.rec_msg = RCIn()
self.diag_agg_msg = DiagnosticArray()
self.diag_toplevel_msg = DiagnosticStatus()
self.vehicle_state = VehicleState()
self.fix_msg = NavSatFix()
def __init__(self):
self.global_pos_target = GlobalPositionTarget()
self.imu = Imu()
self.rc_in = RCIn()
self.rc_override = OverrideRCIn()
# launch node
rospy.init_node('offboard_node', disable_signals=True)
self.mavlink_msg_publisher = rospy.Publisher(mavlink_topic_pub,
Int8,
queue_size=0)
self.mavlink_rc_publisher = rospy.Publisher(mavlink_rc_pub,
RCIn,
queue_size=0)
self.mavlink_rc_override_publisher = rospy.Publisher(mavlink_rc_pub,
OverrideRCIn,
queue_size=0)
rospy.Subscriber(mavlink_rc_sub, RCIn, self.on_rc_in)
rospy.Subscriber(mavlink_topic_sub, GlobalPositionTarget,
self.on_global_pos_target)
rospy.Subscriber(mavlink_imu_sub, Imu, self.on_imu)
self.rate = rospy.Rate(update_rate)
rospy.sleep(1) # wait until everything is running
#!/usr/bin/env python
'''
Thread de seguranca que permite mudar o modo de voo do drone para MANUAL
assim que um toggle no controle remoto e' ativado
'''
import rospy
import mavros_msgs
from mavros_msgs.msg import State
from mavros_msgs.msg import RCIn
import time
drone_state = State()
rc = RCIn()
def rc_callback(data):
global rc
rc = data
def safety_thread():
rate = rospy.Rate(20)
set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
rc_sub = rospy.Subscriber('/mavros/rc/in', mavros_msgs.msg.RCIn,
rc_callback)
while rc.rssi == 0:
rate.sleep()
rospy.loginfo("RC SAFETY THREAD INICIALIZED!!")
while not rospy.is_shutdown():
#print (rc.channels[16])
if (rc.channels[16] == 1998):
def __init__(self, cfg = []):
# Log is state_size + vehicle armed + first letter of vehicle mode + time
self.log = np.zeros([3+sum(state_sizes),10000])
# super(ROSLoadController, self).__init__()
self.n = cfg['number_of_vehicles']
self.pL = np.zeros([1,3,1])
self.vL = np.zeros([1,3,1])
self.RL = np.zeros([1,3,3])
self.oL = np.zeros([1,3,1])
self.pQ = np.zeros([number_of_vehicles,3,1])
self.vQ = np.zeros([number_of_vehicles,3,1])
self.RQ = np.zeros([number_of_vehicles,3,3])
self.oQ = np.zeros([number_of_vehicles,3,1])
self.position_active = True
self.attitude_active = False
self.load_active = False
self.q_input = np.zeros([self.n,4,1])
self.q_input[:,0,0] = 1
self.T_input = np.zeros([self.n,1,1])
self.state = State()
self.rc = RCIn()
self.sub_topics_ready = {
key: False
for key in ['vehicle_odom', 'load_odom', 'state', 'rc']
}
self.mode = number_of_vehicles*['.']
self.armed = np.zeros([self.n,1,1])
# ROS subscribers
for i in range(number_of_vehicles):
self.vehicle_odom_sub = rospy.Subscriber(cfg[i]['vicon'] + '/odom',
Odometry,
self.vehicle_odom_callback,
callback_args=i)
self.state_sub = rospy.Subscriber(cfg[i]['mavros'] + '/mavros/state',
State,
self.state_callback,
callback_args=i)
self.rc_sub = rospy.Subscriber(cfg[0]['mavros'] + '/mavros/rc/in',
RCIn,
self.rc_callback)
print(f"Subscribing to {cfg[0]['mavros'] + '/mavros/rc/in'}")
self.load_odom_sub = rospy.Subscriber(cfg['load']['vicon'] + '/odom',
Odometry,
self.load_odom_callback)
# ROS Publishers
# Attitude
self.att = AttitudeTarget()
self.att_setpoint_pub = [rospy.Publisher(
cfg[i]['mavros'] + '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) for i in range(number_of_vehicles)]
print('publish attitude at: ', cfg[i]['mavros'] + '/mavros/setpoint_raw/attitude')
# 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(
cfg[i]['mavros'] + '/mavros/setpoint_position/local', PoseStamped, queue_size=1) for i in range(number_of_vehicles)]
# send setpoints in separate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
