def __init__(self, ic, node=None):
"""
Init method.
@param ic: The ICE controller.
@param node: The ROS node controller.
"""
self.camera = CameraClient(ic, "drone.Camera", True)
self.cmdvel = CMDVel(ic, "drone.CMDVel")
self.extra = Extra(ic, "drone.Extra")
self.navdata = NavDataClient(ic, "drone.Navdata", True)
self.pose = Pose3DClient(ic, "drone.Pose3D", True)
def __init__(self, ic, node=None):
"""
Init method.
@param ic: The ICE controller.
@param node: The ROS node controller.
"""
self.camera = CameraClient(ic, "drone.Camera", True)
self.cmdvel = CMDVel(ic, "drone.CMDVel")
self.extra = Extra(ic, "drone.Extra")
self.navdata = NavDataClient(ic, "drone.Navdata", True)
self.pose = Pose3DClient(ic, "drone.Pose3D", True)
from parallelIce.pose3dClient import Pose3DClient
from gui.GUI import MainWindow
from PyQt5.QtWidgets import QApplication
import signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
if __name__ == '__main__':
ic = EasyIce.initialize(sys.argv)
cameraCli = CameraClient(ic, "Introrob.Camera", True)
camera = CameraFilter(cameraCli)
navdata = NavDataClient(ic, "Introrob.Navdata", True)
pose = Pose3DClient(ic, "Introrob.Pose3D", True)
cmdvel = CMDVel(ic, "Introrob.CMDVel")
extra = Extra(ic, "Introrob.Extra")
algorithm = MyAlgorithm(camera, navdata, pose, cmdvel, extra)
app = QApplication(sys.argv)
frame = MainWindow()
frame.setCamera(camera)
frame.setNavData(navdata)
frame.setPose3D(pose)
frame.setCMDVel(cmdvel)
frame.setExtra(extra)
frame.setAlgorithm(algorithm)
frame.show()
t2 = ThreadGUI(frame)
t2.daemon = True
from parallelIce.extra import Extra
from parallelIce.pose3dClient import Pose3DClient
from uav_viewer_py.gui.GUI import MainWindow
from PyQt5.QtWidgets import QApplication
import signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
if __name__ == '__main__':
ic = EasyIce.initialize(sys.argv)
camera = CameraClient(ic, "UavViewer.Camera", True)
navdata = NavDataClient(ic, "UavViewer.Navdata", True)
pose = Pose3DClient(ic, "UavViewer.Pose3D", True)
cmdvel = CMDVel(ic, "UavViewer.CMDVel")
extra = Extra(ic, "UavViewer.Extra")
app = QApplication(sys.argv)
frame = MainWindow()
frame.setCamera(camera)
frame.setNavData(navdata)
frame.setPose3D(pose)
frame.setCMDVel(cmdvel)
frame.setExtra(extra)
frame.show()
t2 = ThreadGUI(frame)
t2.daemon = True
t2.start()
sys.exit(app.exec_())
class Drone():
"""
Drone class.
"""
def __init__(self, ic, node=None):
"""
Init method.
@param ic: The ICE controller.
@param node: The ROS node controller.
"""
self.camera = CameraClient(ic, "drone.Camera", True)
self.cmdvel = CMDVel(ic, "drone.CMDVel")
self.extra = Extra(ic, "drone.Extra")
self.navdata = NavDataClient(ic, "drone.Navdata", True)
self.pose = Pose3DClient(ic, "drone.Pose3D", True)
def close(self):
"""
Close communications with servers.
"""
self.camera.stop()
self.navdata.stop()
self.pose.stop()
def color_object_centroid(self):
"""
Return the x,y centroid of a colorful object in the camera.
@return: The (x,y) centroid of the object or None if not found.
"""
# FIXME: set color (manual)
color = BLUE_RANGE
# get image from camera
frame = self.camera.getImage()
# resize the frame
frame = imutils.resize(frame, width=600)
# convert to the HSV color space
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# construct a mask for the color specified
# then perform a series of dilations and erosions
# to remove any small blobs left in the mask
mask = cv2.inRange(hsv, color[0], color[1])
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask and
# initialize the current center
cnts = cv2.findContours(
mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 10:
# draw the circle border
cv2.circle(
frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
# and the centroid
cv2.circle(frame, center, 5, (0, 0, 255), -1)
# show the frame to our screen
cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
return center
def go_up_down(self, direction):
"""
Set the vertical movement of the drone.
@param direction: direction of the move. Options: forward (default), back.
"""
# set default velocity (m/s)
vz = 1.0
if direction == "down":
vz = -vz
# assign velocity
self.cmdvel.setVZ(vz)
# publish movement
self.cmdvel.sendVelocities()
def move(self, direction):
"""
Set the horizontal movement of the drone.
@param direction: direction of the move. Options: forward (default), back.
"""
# set default velocities (m/s)
vx = 5.0
vy = 0.0
# set different direction
if direction == "back":
vx = -vx
elif direction == "left":
vy = float(vx)
vx = 0.0
elif direction == "right":
vy = float(-vx)
vx = 0.0
# assign velocities
self.cmdvel.setVX(vx)
self.cmdvel.setVY(vy)
# publish movement
self.cmdvel.sendVelocities()
def turn(self, direction):
"""
Set the angular velocity.
@param direction: direction of the move. Options: left (default), right.
"""
# set default velocity (m/s)
yaw = 5.0 * math.pi
if direction == "right":
yaw = -yaw
# assign velocity
self.cmdvel.setYaw(yaw)
# publish movement
self.cmdvel.sendVelocities()
def stop(self):
"""
Set all velocities to zero.
"""
self.cmdvel.setVX(0)
self.cmdvel.setVY(0)
self.cmdvel.setVZ(0)
self.cmdvel.setYaw(0)
self.cmdvel.sendVelocities()
def take_off(self):
"""
Send the take off command.
"""
self.extra.takeoff()
time.sleep(1)
def land(self):
"""
Send the land command.
"""
self.extra.land()
time.sleep(1)
class Drone():
"""
Drone class.
"""
def __init__(self, ic, node=None):
"""
Init method.
@param ic: The ICE controller.
@param node: The ROS node controller.
"""
self.camera = CameraClient(ic, "drone.Camera", True)
self.cmdvel = CMDVel(ic, "drone.CMDVel")
self.extra = Extra(ic, "drone.Extra")
self.navdata = NavDataClient(ic, "drone.Navdata", True)
self.pose = Pose3DClient(ic, "drone.Pose3D", True)
def close(self):
"""
Close communications with servers.
"""
self.camera.stop()
self.navdata.stop()
self.pose.stop()
def color_object_centroid(self):
"""
Return the x,y centroid of a colorful object in the camera.
@return: The (x,y) centroid of the object or None if not found.
"""
# FIXME: set color (manual)
color = BLUE_RANGE
# get image from camera
frame = self.camera.getImage()
# resize the frame
frame = imutils.resize(frame, width=600)
# convert to the HSV color space
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# construct a mask for the color specified
# then perform a series of dilations and erosions
# to remove any small blobs left in the mask
mask = cv2.inRange(hsv, color[0], color[1])
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask and
# initialize the current center
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 10:
# draw the circle border
cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255),
2)
# and the centroid
cv2.circle(frame, center, 5, (0, 0, 255), -1)
# show the frame to our screen
cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
return center
def go_up_down(self, direction):
"""
Set the vertical movement of the drone.
@param direction: direction of the move. Options: forward (default), back.
"""
# set default velocity (m/s)
vz = 1.0
if direction == "down":
vz = -vz
# assign velocity
self.cmdvel.setVZ(vz)
# publish movement
self.cmdvel.sendVelocities()
def move(self, direction):
"""
Set the horizontal movement of the drone.
@param direction: direction of the move. Options: forward (default), back.
"""
# set default velocities (m/s)
vx = 5.0
vy = 0.0
# set different direction
if direction == "back":
vx = -vx
elif direction == "left":
vy = float(vx)
vx = 0.0
elif direction == "right":
vy = float(-vx)
vx = 0.0
# assign velocities
self.cmdvel.setVX(vx)
self.cmdvel.setVY(vy)
# publish movement
self.cmdvel.sendVelocities()
def turn(self, direction):
"""
Set the angular velocity.
@param direction: direction of the move. Options: left (default), right.
"""
# set default velocity (m/s)
yaw = 5.0 * math.pi
if direction == "right":
yaw = -yaw
# assign velocity
self.cmdvel.setYaw(yaw)
# publish movement
self.cmdvel.sendVelocities()
def stop(self):
"""
Set all velocities to zero.
"""
self.cmdvel.setVX(0)
self.cmdvel.setVY(0)
self.cmdvel.setVZ(0)
self.cmdvel.setYaw(0)
self.cmdvel.sendVelocities()
def take_off(self):
"""
Send the take off command.
"""
self.extra.takeoff()
time.sleep(1)
def land(self):
"""
Send the land command.
"""
self.extra.land()
time.sleep(1)
from parallelIce.extra import Extra
from parallelIce.pose3dClient import Pose3DClient
from gui.GUI import MainWindow
from PyQt5.QtWidgets import QApplication
import signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
if __name__ == '__main__':
ic = EasyIce.initialize(sys.argv)
camera = CameraClient(ic, "UAVViewer.Camera", True)
navdata = NavDataClient(ic, "UAVViewer.Navdata", True)
pose = Pose3DClient(ic, "UAVViewer.Pose3D", True)
cmdvel = CMDVel(ic, "UAVViewer.CMDVel")
extra = Extra(ic, "UAVViewer.Extra")
app = QApplication(sys.argv)
frame = MainWindow()
frame.setCamera(camera)
frame.setNavData(navdata)
frame.setPose3D(pose)
frame.setCMDVel(cmdvel)
frame.setExtra(extra)
frame.show()
t2 = ThreadGUI(frame)
t2.daemon = True
t2.start()
sys.exit(app.exec_())