def __init__(self):
rospy.init_node('msgControl')
self.rate = rospy.Rate(20)
self.enable = False
self.sysState = None
self.homePos = None
self.gotMission = False
self.loiterMsg = mavSP.PoseStamped()
self.pylonNavMsg = None
self.inspectMsg = None
self.setpoint = mavSP.PoseStamped()
self.curLocalPos = mavSP.PoseStamped()
self.setpointPub = mavSP.get_pub_position_local(queue_size=1)
self.wpCompletePub = rospy.Publisher(wpCompleteTopic,
Int8,
queue_size=1)
self.homeGPSPub = rospy.Publisher(homeReqPub, Bool, queue_size=1)
# root_framework Subs
rospy.Subscriber(onB_StateSub, String, self.onStateChange)
rospy.Subscriber('/onboard/messageEnable', Bool, self.handlerEnable)
rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
mavSP.PoseStamped, self._cb_localPosUpdate)
rospy.Subscriber(mavros.get_topic('home_position', 'home'),
mavros_msgs.msg.HomePosition, self._cb_onHomeUpdate)
# pilot subs
rospy.Subscriber(loiterSub, mavSP.PoseStamped, self.pilot_loiterMsg)
rospy.Subscriber(missionSub, NavSatFix, self.pilot_pylonNavMsg)
rospy.Subscriber(inspectSub, mavSP.PoseStamped,
self._onInspectPosUpdate)
def blocked_yaw(self, yaw=120):
throttle_pid = pid_controller.PID(P=1.0,
I=0.0,
D=0.5,
Integrator_max=0.5,
Integrator_min=0.0)
throttle_pid.setPoint(self.current_alt)
"attempting to maintain alt: ", self.current_alt
time.sleep(2)
while True:
att_pub = SP.get_pub_attitude_pose(queue_size=10)
thd_pub = SP.get_pub_attitude_throttle(queue_size=10)
#while not rospy.is_shutdown():
pose = SP.PoseStamped(header=SP.Header(stamp=rospy.get_rostime()))
q = quaternion_from_euler(0, 0, 90)
pose.pose.orientation = SP.Quaternion(*q)
throt = throttle_pid.update(self.current_alt)
#if throt > 0.6:
# throt = 0.6
#if throt < 0.4:
# throt = 0.6
if self.attitude_publish or True:
att_pub.publish(pose)
thd_pub.publish(data=throt)
print "pose orientation: ", throt
def publish_orientation(self, ang=180):
att_pub = SP.get_pub_attitude_pose(queue_size=10)
#thd_pub = SP.get_pub_attitude_throttle(queue_size=10)
#vel_pub = SP.get_pub_velocity_cmd_vel(queue_size=10)
rate = rospy.Rate(10) # 10hz
## msg = SP.PoseStamped(
## header=SP.Header(
## frame_id="base_footprint", # no matter, plugin don't use TF
## stamp=rospy.Time.now()), # stamp should update
## )
while not rospy.is_shutdown():
#msg.pose.position.x = self.x
#msg.pose.position.y = self.y
#msg.pose.position.z = self.z
pose = SP.PoseStamped(header=SP.Header(stamp=rospy.get_rostime()))
q = quaternion_from_euler(0, 0, ang)
pose.pose.orientation = SP.Quaternion(*q)
#thd_pub.publish(data=self.throttle_update)
#thd_pub.publish(data=0.6)
att_pub.publish(pose)
print "pose orientation: ", self.throttle_update
def __init__(self):
rospy.init_node('examplePilot')
self.subState = 'GPS'
self.GPSMode = True
self.enable = False
self.rate = rospy.Rate(20)
self.exampleGPSPub = rospy.Publisher(targetWP_GPS,
mavSP.PoseStamped,
queue_size=5)
self.exampleATTIPub = rospy.Publisher(targetWP_Atti,
mavros_msgs.msg.AttitudeTarget,
queue_size=5)
self.pilotSubstatePub = rospy.Publisher(onB_substateSub,
String,
queue_size=1)
rospy.Subscriber(onB_StateSub, String, self.onStateChange)
rospy.Subscriber(
mavros.get_topic('local_position',
'pose'), mavSP.PoseStamped, self.onPositionChange
) # uses mavros.get topic to find the name of the current XYZ position of the drone
self.currUAVPos = mavSP.PoseStamped() # used for GPS setpoints
self.msgSPAtti = mavSP.TwistStamped() # used for Attitude setpoints
rospy.loginfo('examplePilot Initialised.')
def navigate(self):
rate = rospy.Rate(10) # 10hz
msg = SP.PoseStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
msg.pose.position.x = self.x
msg.pose.position.y = self.y
msg.pose.position.z = self.z
quaternion = quaternion_from_euler(0, 0, self.yaw)
#msg.pose.orientation.x = quaternion[0]
#msg.pose.orientation.y = quaternion[1]
#msg.pose.orientation.z = quaternion[2]
#msg.pose.orientation.w = quaternion[3]
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 180 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, self.yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def publish_pose(self):
self.pub = SP.get_pub_position_local(queue_size=10)
rate = rospy.Rate(10) # 10hz
msg = SP.PoseStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
#msg.pose.position.x = self.x
#msg.pose.position.y = self.y
#msg.pose.position.z = self.z
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 0.0
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = self.yaw # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
if self.publish == True:
self.pub.publish(msg)
print "publishing position"
rate.sleep()
def setBearing(self, bearing=0.0):
orientAdj = quaternion_from_euler(0, 0, radians(bearing))
desiredBearing = mavSP.PoseStamped()
desiredBearing.pose.orientation = Quaternion(*orientAdj)
return desiredBearing.pose.orientation
def __init__(self):
rospy.init_node('droneCMD_node')
self.rate = rospy.Rate(20)
self.first = True
self.homeCoord = None
self.gpsPos = NavSatFix()
self.curPos = mavSP.PoseStamped()
self.loiterPos = mavSP.PoseStamped()
self.uavState = mavros_msgs.msg.State()
self.setPoint = mavSP.PoseStamped()
self.companion_has_control = False
self.droneArmed = False
self.isAirbourne = False
self.sysState = None
# Publishers/Subscribers
self.statePub = rospy.Publisher(onB_StateSub, String, queue_size=1)
self.spLocalPub = mavSP.get_pub_position_local(queue_size=5)
self.homePub = rospy.Publisher(homePubTopic,
mavros_msgs.msg.HomePosition,
queue_size=1)
self.messageHandlerPub = rospy.Publisher('/onboard/messageEnable',
Bool,
queue_size=1)
# Services
self.setMode = rospy.ServiceProxy('/mavros/set_mode',
mavros_msgs.srv.SetMode)
rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State,
self._cb_uavState)
rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
mavSP.PoseStamped, self._cb_localPos)
rospy.Subscriber(mavros.get_topic('global_position', 'global'),
NavSatFix, self._cb_SatFix)
rospy.Subscriber(mavros.get_topic('home_position', 'home'),
mavros_msgs.msg.HomePosition, self._cb_HomeUpdate)
rospy.Subscriber(keySub, Int8, self._cb_onKeypress)
rospy.Subscriber(pilotStateSub, pilot_cb, self._pilotStateUpdate)
rospy.Subscriber(homeReqPub, Bool, self.onHomeRequest)
def __init__(self):
rospy.init_node('loiterPilot')
self.rate = rospy.Rate(20)
self.enable = False
rospy.Subscriber(onB_StateSub, String, self.onStateChange)
rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
mavSP.PoseStamped, self.onPositionChange)
rospy.Subscriber(keySub, Int8, self._cb_onKeypress)
# self.targetPub = rospy.Publisher
# self.loiterPub = mavSP.get_pub_position_local(queue_size=5)
self.loiterPub = rospy.Publisher(targetWP,
mavSP.PoseStamped,
queue_size=5)
self.loiterPos = mavSP.PoseStamped()
self.curPos = mavSP.PoseStamped()
def navigate(self):
rate = rospy.Rate(10) # 10hz
while True:
msg = SP.PoseStamped(header=SP.Header(frame_id='',
stamp=rospy.Time.now()), )
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 3.0
rospy.loginfo(msg)
self.pub.publish(msg)
def setBearing(self, bearing=0.0):
orientAdj = quaternion_from_euler(0, 0, radians(bearing))
desiredBearing = mavSP.PoseStamped()
desiredBearing.pose.orientation = Quaternion(*orientAdj)
# desiredBearing.pose.orientation.x = orientAdj[0]
# desiredBearing.pose.orientation.y = orientAdj[1]
# desiredBearing.pose.orientation.z = orientAdj[2]
# desiredBearing.pose.orientation.w = orientAdj[3]
return desiredBearing.pose.orientation
def __init__(self):
rospy.init_node('loiterPilot')
self.rate = rospy.Rate(20)
self.enable = False
self.uavHead = 0.0
rospy.Subscriber(onB_StateSub, String, self.onStateChange)
rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
mavSP.PoseStamped, self.onPositionChange)
rospy.Subscriber(mavros.get_topic('global_position', 'compass_hdg'),
Float64, self.onHeadingUpdate)
rospy.Subscriber(keySub, Int8, self._cb_onKeypress)
# self.targetPub = rospy.Publisher
# self.loiterPub = mavSP.get_pub_position_local(queue_size=5)
self.headingMode = False
self.loiterPub = rospy.Publisher(targetWP,
mavSP.PoseStamped,
queue_size=5)
self.loiterPos = mavSP.PoseStamped()
self.curPos = mavSP.PoseStamped()
rospy.loginfo('loiterPilot Ready')
def navigate(self):
msg = SP.PoseStamped(
header=SP.Header(
frame_id="waypoint_to_go", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
msg.pose.position.x = self.x
msg.pose.position.y = self.y
msg.pose.position.z = self.z
yaw = radians(self.yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def __init__(self):
rospy.init_node('D4Emaster_node')
self.rate = rospy.Rate(20)
# important variables
self.MH_enabled = False
self.sysState = 'idle'
self.MAVROS_State = mavros_msgs.msg.State()
self.isAirbourne = False
self.uavGPSPos = None
self.uavLocalPos = mavSP.PoseStamped()
self.uavHdg = None
#"killSwitch"
rospy.Subscriber(isolatedSub, Bool, self.onKillSwitch)
# Subscribers
rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State,
self._cb_uavState)
rospy.Subscriber(mavros.get_topic('global_position', 'global'),
NavSatFix, self._cb_SatFix)
rospy.Subscriber(mavros.get_topic('global_position', 'compass_hdg'),
Float64, self._cb_headingUpdate)
rospy.Subscriber(keySub, Int8, self._cb_onKeypress)
# Publishers
self.statePub = rospy.Publisher(onB_StateSub, String, queue_size=1)
self.enableMHPub = rospy.Publisher(mh_enableSub, Bool, queue_size=1)
self.llpPub = rospy.Publisher(isolatedSub, Bool, queue_size=1)
self.spLocalPub = mavSP.get_pub_position_local(queue_size=5)
# Services
self.setMode = rospy.ServiceProxy('/mavros/set_mode',
mavros_msgs.srv.SetMode)
self.enableTakeoff = rospy.ServiceProxy('/ mavros/cmd/takeoff',
mavros_msgs.srv.CommandTOL)
# Message members
self._mavrosHandshake()
def navigate(self):
rate = rospy.Rate(10)
msg = setpoint.PoseStamped(
header=setpoint.Header(
frame_id="map", # isn't used anyway
stamp=rospy.Time.now()), )
while not rospy.is_shutdown():
#msg.pose.position.x = self.y
#msg.pose.position.y = self.x
#msg.pose.position.z = - self.z
msg.pose.position.x = self.x
msg.pose.position.y = self.y
msg.pose.position.z = self.z
self.pub.publish(msg)
rate.sleep()
def pilot_pylonNavMsg(self, msg):
# print(msg)
x, y, z = self.gpsToLocal(msg)
orient = math.atan2(y, x)
if x > 0:
orientation = Quaternion(
*quaternion_from_euler(0, 0, orient + math.pi))
else:
orientation = Quaternion(*quaternion_from_euler(0, 0, -orient))
tmpSP = mavSP.PoseStamped()
tmpSP.pose.position.x = y
tmpSP.pose.position.y = x
tmpSP.pose.position.z = z
tmpSP.pose.orientation = orientation
self.pylonNavMsg = tmpSP
self.sysState = 'mission'
self.gotMission = True
def altitudeCheck(self):
preMsg = mavSP.PoseStamped()
altCheck = False
# print "curPose:", self.curLocalPos.pose
# print "setPose:", self.setpoint.pose
if abs(self.curLocalPos.pose.position.z -
self.setpoint.pose.position.z) > 0.5:
preMsg.pose.position.x = self.curLocalPos.pose.position.x
preMsg.pose.position.y = self.curLocalPos.pose.position.y
preMsg.pose.position.z = self.setpoint.pose.position.z
altCheck = False
else:
preMsg.pose.position.x = self.setpoint.pose.position.x
preMsg.pose.position.y = self.setpoint.pose.position.y
preMsg.pose.position.z = self.setpoint.pose.position.z
altCheck = True
preMsg.pose.orientation = self.setpoint.pose.orientation
return altCheck, preMsg
def get_pilotMsg(self):
outwardMsg = mavSP.PoseStamped()
outwardMsg = self.loiterMsg
if self.sysState == 'loiter':
outwardMsg = self.loiterMsg
if self.sysState == 'missionHold':
outwardMsg = self.loiterMsg
if self.sysState == 'mission':
if (self.pylonNavMsg == None) or (self.gotMission == False):
outwardMsg = self.loiterMsg
else:
outwardMsg = self.pylonNavMsg
if self.sysState == 'inspect':
outwardMsg = self.inspectMsg
pass
self.setpoint = outwardMsg
def navigate(self):
""" Continuously publishes position target to the flight controller"""
rate = rospy.Rate(8) # in Hz
rate_mode_check = rospy.Rate(20)
message_pos = setpoint.PoseStamped(
header=setpoint.Header(
frame_id="base_footprint", # no matter, plugin doesn't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
message_pos.pose.position.x = self.setpoint_x
message_pos.pose.position.y = self.setpoint_y
message_pos.pose.position.z = self.setpoint_z
yaw = 0
quaternion = quaternion_from_euler(0, 0, yaw)
message_pos.pose.orientation = setpoint.Quaternion(*quaternion)
self.setpoint_position.publish(message_pos)
rate.sleep()
def control_loop(self):
rate = rospy.Rate(self.rate) # 10hz
msg = SP.PoseStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
msg.pose.position.x = self.x_ref
msg.pose.position.y = self.y_ref
msg.pose.position.z = self.z_ref
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = 0 # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def navigate_position(self):
msg_set_pos = SP.PoseStamped(
header=SP.Header(
frame_id="waypoint_to_go", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
while not rospy.is_shutdown():
if not self.activated:
break
msg_set_pos.pose.position.x = self.x
msg_set_pos.pose.position.y = self.y
msg_set_pos.pose.position.z = self.z
yaw = math.radians(self.yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg_set_pos.pose.orientation = SP.Quaternion(*quaternion)
# rospy.loginfo("there is life on navigate")
self.pub_setpoint.publish(msg_set_pos)
rate.sleep()
def navigate(self):
rate = rospy.Rate(10) # 10hz
command =Twist()
msg = SP.PoseStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
msg.pose.position.x = self.x
msg.pose.position.y = self.y
msg.pose.position.z = self.z
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = self.ya # North: ya=0
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
command.angular.z = kp*yaw #yaw pose control
self.pub_cmd_vel.publish(command) #publish yaw control
self.pub_pos.publish(msg) #publish position control
rate.sleep()
from tf.transformations import euler_from_quaternion
from std_msgs.msg import Int16MultiArray
from geometry_msgs.msg import Vector3
import math
import time
from pid_controller.pid import PID
def signal_handler(signal, frame):
print('You pressed Ctrl+C!')
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
current_pose = SP.PoseStamped()
global new_lower_pos
new_lower_pos = 10000
global pixel_pos
pixel_pos = [0, 0]
UAV_state = mavros_msgs.msg.State()
global last_request
global set_mode
def state_callback(topic):
UAV_state.armed = topic.armed
UAV_state.connected = topic.connected
def control_pid(self):
msg = SP.TwistStamped(
header=SP.Header(
frame_id="Drone_Vel_setpoint", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
ref_pose_msg = SP.PoseStamped(
header=SP.Header(
frame_id="setpoint_position", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
fuz_msg = fuzzy_msg(
header=SP.Header(
frame_id="fuzzy_values", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
smc_msg = vehicle_smc_gains_msg(
header=SP.Header(
frame_id="smc_values", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
while not rospy.is_shutdown():
if not self.activated:
break
if self.fuzzyfy:
# TODO: fuzzy com y e z
rospy.loginfo_once("initalized fuzzyfication for the fist time")
if not self.defuzzed.P_X == 0:
rospy.loginfo_once("first value of P_X modified by fuzzy system")
rospy.loginfo_once("P_x = " + str(self.defuzzed.P_X))
global KPx
KPx += self.defuzzed.P_X/SOFTNESS
if KPx <=0.6: KPx=0.6
if KPx >=5.6: KPx=5.6
global KIx
KIx += self.defuzzed.I_X/SOFTNESS
if KIx <= 0.3: KIx = 0.3
if KIx >= 4.5: KIx = 4.5
global KDx
KDx += self.defuzzed.D_X/SOFTNESS
if KDx <= 0.1: KDx = 0.1
if KDx >= 0.8: KDx = 0.8
self.error.x.act = self.x - DronePose.x
self.error.y.act = self.y - DronePose.y
self.error.z.act = self.z - DronePose.z
# rospy.loginfo(KPx)
PX = self.error.x.act * KPx
PY = self.error.y.act * KPy
PZ = self.error.z.act * KPz
_time_bet_run = (rate.sleep_dur.nsecs / 1000000000.0)
DX = ((self.error.x.Deri) * float(KDx)) / _time_bet_run
DY = ((self.error.y.Deri) * float(KDy)) / _time_bet_run
DZ = ((self.error.z.Deri) * float(KDz)) / _time_bet_run
self.error.x.Intg = self.error.x.act * KIx * _time_bet_run
IX = self.error.x.Intg
self.error.y.Intg = self.error.y.act * KIy * _time_bet_run
IY = self.error.y.Intg
self.error.z.Intg = self.error.z.act * KIz * _time_bet_run
IZ = self.error.z.Intg
# rospy.loginfo(IY)
self.x_vel = PX + DX + IX
self.y_vel = PY + DY + IY
self.z_vel = PZ + DZ + IZ
fuz_msg.Error = self.error.x.act
fuz_msg.D_Error = self.error.x.Deri
fuz_msg.P_val = KPx
fuz_msg.I_val = KIx
fuz_msg.D_val = KDx
ref_pose_msg.pose.position.x = setpoint_vel.x
ref_pose_msg.pose.position.y = setpoint_vel.y
ref_pose_msg.pose.position.z = setpoint_vel.z
msg.twist.linear.x = self.x_vel
msg.twist.linear.y = self.y_vel
msg.twist.linear.z = self.z_vel
smc_msg.k_gains = K_roll, K_pitch, K_yaw
smc_msg.beta_gains = B_roll, B_pitch, B_yaw
smc_msg.lambda_gains = L_roll, L_pitch, L_yaw
fuz_msg.header.stamp = rospy.Time.now()
self.pub_fuz.publish(fuz_msg)
ref_pose_msg.header.stamp = rospy.Time.now()
self.pub_refpos.publish(ref_pose_msg)
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
smc_msg.header.stamp = rospy.Time.now()
self.pub_smc.publish(smc_msg)
rate.sleep()
