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 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 __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 __init__(self):
self.pos_x = 0.0
self.pos_y = 0.0
self.pos_z = 0.0
self.setpoint_x = 0.0
self.setpoint_y = 0.0
self.setpoint_z = 0.0
# publisher for mavros/setpoint_position/pose
self.setpoint_position = setpoint.get_pub_position_local(queue_size=1)
# subscriber for mavros/local_position/pose
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
setpoint.PoseStamped,
self._hasReached,
queue_size=1)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
# TODO(simon): Clean this up.
self.done = False
self.done_evt = threading.Event()
def __init__(self):
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.currentPoseX = 0
self.currentPoseY = 0
self.currentPoseZ = 0
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
self.waypointsub = rospy.Subscriber("/uav_1/waypoint",
SP.PoseStamped,
self.updateposition,
queue_size=1)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
# TODO(simon): Clean this up.
self.done = False
self.done_evt = threading.Event()
def __init__(self, actionServer, goal):
'''Initialize ros publisher, ros subscriber'''
rospack = rospkg.RosPack()
packagePath = rospack.get_path('kuri_object_tracking')
with open(packagePath+'/config/colors.yaml', 'r') as stream:
try:
config_yaml = yaml.load(stream)
self.H_red = config_yaml.get('H_red')
self.S_red = config_yaml.get('S_red')
self.V_red = config_yaml.get('V_red')
self.H_blue = config_yaml.get('H_blue')
self.S_blue = config_yaml.get('S_blue')
self.V_blue = config_yaml.get('V_blue')
self.H_green = config_yaml.get('H_green')
self.S_green = config_yaml.get('S_green')
self.V_green = config_yaml.get('V_green')
except yaml.YAMLError as exc:
print(exc)
self.navigate_started = False
self.bridge = CvBridge()
self.actionServer = actionServer
self.obstacles = Objects()
self.image_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/image', Image)
self.info_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/camera_info', CameraInfo)
#self.image_sub = message_filters.Subscriber('/usb_cam/image_raw', Image)
#self.info_sub = message_filters.Subscriber('/usb_cam/camera_info', CameraInfo)
#self.outpub = rospy.Publisher('/uav_'+str(goal.uav_id)+'/downward_cam/camera/detection_image',Image, queue_size=100, latch=True)
self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10)
self.ts.registerCallback(self.callback)
mavros.set_namespace('/uav_'+str(goal.uav_id)+'/mavros')
self.currentPoseX = -1
self.currentPoseY = -1
self.currentPoseZ = -1
self.pub = SP.get_pub_position_local(queue_size=10)
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.updatePosition)
self.image = None
self.left_image = None
self.right_image = None
self.data_small = np.genfromtxt(packagePath+'/config/small.dat', np.float32, delimiter=',')
self.data_big = np.genfromtxt(packagePath+'/config/big.dat', np.float32, delimiter=',')
self.data_dropzone = np.genfromtxt(packagePath+'/config/dropzone.dat', np.float32, delimiter=',')
self.samples = np.reshape(self.data_small, (-1, 7))
self.samples_big = np.reshape(self.data_big, (-1, 7))
self.samples_dropzone = np.reshape(self.data_dropzone, (-1, 7))
self.objects_count = 0
self.frame = 0
self.new_objects = Objects()
self.new_objects_count = 0
self.navigating = False
self.done = False
self.edgeThresholdSize = 0.1
self.width = 1600
self.height = 1200
def init(self, _x, _y, _z):
self.x = _x
self.y = _y
self.z = _z
self.yaw_degrees = 0
self.done_evt = threading.Event()
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached)
def __init__(self):
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=1)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
try:
thread.start_new_thread(self.navigate, ())
except:
print("Error: Unable to start thread")
self.done = False
self.done_evt = threading.Event()
def __init__(self):
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.pub = SP.get_pub_position_local(queue_size=10)
self.sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached)
try:
thread.start_new_thread( self.navigate, () )
except:
fault("Error: Unable to start the thread")
self.done = False
self.done_evt = threading.Event()
def __init__(self):
self.x = 0.0
self.y = 0.0
self.z = 0.0
# publisher for mavros/setpoint_position/local
self.pub = setpoint.get_pub_position_local(queue_size=10) # warum 10?
# subsrciber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
setpoint.PoseStamped, self.reached)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
self.done = False
self.done_evt = threading.Event()
def __init__(self):
self.x = 0.0
self.y = 0.0
self.z = 0.0
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic("local_position", "pose"), SP.PoseStamped, self.reached)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
# TODO(simon): Clean this up.
self.done = False
self.done_evt = threading.Event()
def __init__(self, actionServer):
self.actionServer = actionServer
self.done = False
self.done_evt = threading.Event()
self.isExploring = False
self.progress = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.currentPoseX = 0
self.currentPoseY = 0
self.currentPoseZ = 0
self.navigating = False
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
self.objects_map = ObjectsMap()
self.client = actionlib.SimpleActionClient('MappingAction',
MappingAction)
client = self.client
#client = self.actionServer.client
print "Waiting for mapping server"
client.wait_for_server()
goal = TrackingGoal()
goal.uav_id = 3
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
print "Result:"
self.objects_map = client.get_result().objects_map
print self.objects_map
self.sub = rospy.Subscriber("MappingAction/feedback", ObjectsMap,
self.callback)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
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 __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,
tollerance=0.05,
rate=10,
topic='/mavros/local_position/pose',
name='pos_controller'):
ControlPosition.__init__(self, tollerance, rate, topic, name)
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
try:
thread.start_new_thread(self.control_loop, ())
except:
fault("Error: Unable to start thread")
# TODO(simon): Clean this up.
self.done = False
self.done_evt = threading.Event()
def __init__(self, sonar_obs_present=[], jMAVSim=False ):
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.yaw = 0.0
self.jMAVSim = jMAVSim
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
try:
#thread.start_new_thread( self.navigate, () )
navigation_thread = threading.Thread(target=self.navigate)
navigation_thread.start()
except:
print "Error: Unable to start thread"
self.sonar_obs_present = sonar_obs_present
self.obs_detected = False
#current coordinates
self.curr_x = 0.0
self.curr_y = 0.0
self.curr_z = 0.0
self.x_home = 0
self.y_home = 0
self.z_home = 0
#coordinates for original setpoint, stored to continue to desired setpoint if interrupted
self.x_final_dest = 0.0
self.y_final_dest = 0.0
self.z_final_dest = 0.0
self.yaw_final_dest = 0.0
self.done = False
self.done_evt = threading.Event()
def __init__(self):
self.done = False
self.done_evt = threading.Event()
self.isExploring = False
self.progress = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.currentPoseX = 0
self.currentPoseY = 0
self.currentPoseZ = 0
self.navigating = False
mavros.set_namespace('/uav_1/mavros')
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
self.objects_map = ObjectsMap()
self.client = actionlib.SimpleActionClient('TrackingAction',
TrackingAction)
#client = self.client
#client = self.actionServer.client
print "Waiting for tracking server"
self.client.wait_for_server()
self.goal = TrackingGoal()
self.goal.uav_id = 1
self.client.send_goal(self.goal)
print "Waiting for result"
self.client.wait_for_result()
print "Result:"
self.objects = self.client.get_result().tracked_objects.objects
print self.objects
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
def __init__(self,actionServer):
self.actionServer = actionServer
self.done = False
self.done_evt = threading.Event()
self.isExploring = False
self.progress = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.currentPoseX = 0
self.currentPoseY = 0
self.currentPoseZ = 0
self.navigating = False
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
self.objects_map = ObjectsMap()
self.client = actionlib.SimpleActionClient('MappingAction', MappingAction)
client = self.client
#client = self.actionServer.client
print "Waiting for mapping server"
client.wait_for_server()
goal = TrackingGoal()
goal.uav_id = 3
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
print "Result:"
self.objects_map = client.get_result().objects_map
print self.objects_map
self.sub = rospy.Subscriber("MappingAction/feedback",ObjectsMap, self.callback)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
def __init__(self):
self.done = False
self.done_evt = threading.Event()
self.isExploring = False
self.progress = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.currentPoseX = 0
self.currentPoseY = 0
self.currentPoseZ = 0
self.navigating = False
mavros.set_namespace('/uav_1/mavros')
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
self.objects_map = ObjectsMap()
self.client = actionlib.SimpleActionClient('TrackingAction', TrackingAction)
#client = self.client
#client = self.actionServer.client
print "Waiting for tracking server"
self.client.wait_for_server()
self.goal = TrackingGoal()
self.goal.uav_id = 1
self.client.send_goal(self.goal)
print "Waiting for result"
self.client.wait_for_result()
print "Result:"
self.objects =self.client.get_result().tracked_objects.objects
print self.objects
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
def __init__(self):
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.currentPoseX = 0
self.currentPoseY = 0
self.currentPoseZ = 0
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
self.waypointsub = rospy.Subscriber("/uav_1/waypoint", SP.PoseStamped, self.updateposition, queue_size=1)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
self.done = False
self.done_evt = threading.Event()
def __init__(self):
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.ya = 0.0
# publisher for mavros/setpoint_position/local
self.pub_pos = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub_pos = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
# publisher for cmd_vel
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
# TODO(simon): Clean this up.
self.done = False
self.done_evt = threading.Event()
