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()