class Mouse:
def __init__(self):
self.mouse = DroneWrapper(name="rqt", ns="mouse/")
self.reset_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
# Explicit initialization functions
# Class method, so user can call it without instantiation
@classmethod
def initRobot(cls):
new_instance = cls()
return new_instance
def takeoff(self):
self.mouse.takeoff(h=5)
def start_mouse(self, path_level):
self.takeoff()
self.thread = StoppableThread(target=self.autonomous_mouse,
args=[path_level])
self.thread.start()
def land(self):
self.mouse.land()
def stop_mouse(self):
self.thread.stop()
self.mouse.set_cmd_mix(vx=0,
vy=0,
z=self.mouse.get_position()[2],
az=0)
def reset_mouse(self):
self.thread.stop()
self.mouse.land()
req = ModelState()
req.model_name = "iris_red"
req.pose.position.x = 2.0
req.pose.position.y = 0.0
req.pose.position.z = 0.05
req.pose.orientation.x = 0.0
req.pose.orientation.y = 0.0
req.pose.orientation.z = 0.0
req.pose.orientation.w = 1.0
req.twist.linear.x = 0.0
req.twist.linear.y = 0.0
req.twist.linear.z = 0.0
req.twist.angular.x = 0.0
req.twist.angular.y = 0.0
req.twist.angular.z = 0.0
self.reset_state(req)
def autonomous_mouse(self, path_level):
v = path_obfuscated(path_level)
if v is not None:
vx, vy, vz, yaw = v
self.mouse.set_cmd_vel(vx, vy, vz, yaw)
else:
print("[Mouse] Path {} not available".format(path_level))
class HAL:
IMG_WIDTH = 320
IMG_HEIGHT = 240
def __init__(self):
rospy.init_node("HAL")
self.image = None
self.drone = DroneWrapper(name="rqt")
# Explicit initialization functions
# Class method, so user can call it without instantiation
@classmethod
def initRobot(cls):
new_instance = cls()
return new_instance
# Get Image from ROS Driver Camera
def get_frontal_image(self):
image = self.drone.get_frontal_image()
image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
return image_rgb
def get_ventral_image(self):
image = self.drone.get_ventral_image()
image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
return image_rgb
def get_position(self):
pos = self.drone.get_position()
return pos
def get_velocity(self):
vel = self.drone.get_velocity()
return vel
def get_yaw_rate(self):
yaw_rate = self.drone.get_yaw_rate()
return yaw_rate
def get_orientation(self):
orientation = self.drone.get_orientation()
return orientation
def get_roll(self):
roll = self.drone.get_roll()
return roll
def get_pitch(self):
pitch = self.drone.get_pitch()
return pitch
def get_yaw(self):
yaw = self.drone.get_yaw()
return yaw
def get_landed_state(self):
state = self.drone.get_landed_state()
return state
def set_cmd_pos(self, x, y, z, yaw):
self.drone.set_cmd_pos(x, y, z, yaw)
def set_cmd_vel(self, vx, vy, vz, yaw_rate):
self.drone.set_cmd_vel(vx, vy, vz, yaw_rate)
def set_cmd_mix(self, vx, vy, z, yaw_rate):
self.drone.set_cmd_mix(vx, vy, z, yaw_rate)
def takeoff(self, h=3):
self.drone.takeoff(h)
def land(self):
self.drone.land()
class HAL:
IMG_WIDTH = 320
IMG_HEIGHT = 240
def __init__(self):
rospy.init_node("HAL")
self.image = None
self.cat = DroneWrapper(name="rqt", ns="cat/")
self.mouse = DroneWrapper(name="rqt", ns="mouse/")
# Explicit initialization functions
# Class method, so user can call it without instantiation
@classmethod
def initRobot(cls):
new_instance = cls()
return new_instance
def start_mouse(self, path_level):
print("MOUSE", path_level)
self.mouse.takeoff(h=5)
# while True:
# v = path_obfuscated(path_level)
# if v is not None:
# vx, vy, vz, yaw = v
# self.mouse.set_cmd_vel(vx, vy, vz, yaw)
# else:
# print("[Mouse] Path {} not available".format(path_level))
# break
# Get Image from ROS Driver Camera
def getFrontalImage(self):
image = self.cat.get_frontal_image()
image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
return image_rgb
def getVentralImage(self):
image = self.cat.get_ventral_image()
image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
return image_rgb
def get_position(self):
pos = self.cat.get_position()
return pos
def get_velocity(self):
vel = self.cat.get_velocity()
return vel
def get_yaw_rate(self):
yaw_rate = self.cat.get_yaw_rate()
return yaw_rate
def get_orientation(self):
orientation = self.cat.get_orientation()
return orientation
def get_roll(self):
roll = self.cat.get_roll()
return roll
def get_pitch(self):
pitch = self.cat.get_pitch()
return pitch
def get_yaw(self):
yaw = self.cat.get_yaw()
return yaw
def get_landed_state(self):
state = self.cat.get_landed_state()
return state
def set_cmd_pos(self, x, y, z, yaw):
self.cat.set_cmd_pos(x, y, z, yaw)
def set_cmd_vel(self, vx, vy, vz, yaw_rate):
self.cat.set_cmd_vel(vx, vy, vz, yaw_rate)
def set_cmd_mix(self, vx, vy, z, yaw_rate):
self.cat.set_cmd_mix(vx, vy, z, yaw_rate)
def takeoff(self, h=3):
self.cat.takeoff(h)
def land(self):
self.cat.land()
class HAL:
IMG_WIDTH = 320
IMG_HEIGHT = 240
def __init__(self):
rospy.init_node("HAL")
self.image = None
self.drone = DroneWrapper(name="rqt")
# Explicit initialization functions
# Class method, so user can call it without instantiation
@classmethod
def initRobot(cls):
new_instance = cls()
return new_instance
# Get Image from ROS Driver Camera
def get_frontal_image(self):
image = self.drone.get_frontal_image()
image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
return image_rgb
def get_ventral_image(self):
image = self.drone.get_ventral_image()
image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
return image_rgb
def get_position(self):
pos = self.drone.get_position()
return pos
def get_velocity(self):
vel = self.drone.get_velocity()
return vel
def get_yaw_rate(self):
yaw_rate = self.drone.get_yaw_rate()
return yaw_rate
def get_orientation(self):
orientation = self.drone.get_orientation()
return orientation
def get_roll(self):
roll = self.drone.get_roll()
return roll
def get_pitch(self):
pitch = self.drone.get_pitch()
return pitch
def get_yaw(self):
yaw = self.drone.get_yaw()
return yaw
def get_landed_state(self):
state = self.drone.get_landed_state()
return state
def set_cmd_pos(self, x, y, z, yaw):
self.drone.set_cmd_pos(x, y, z, yaw)
def set_cmd_vel(self, vx, vy, vz, yaw_rate):
self.drone.set_cmd_vel(vx, vy, vz, yaw_rate)
def set_cmd_mix(self, vx, vy, z, yaw_rate):
self.drone.set_cmd_mix(vx, vy, z, yaw_rate)
def takeoff(self, h=3):
self.drone.takeoff(h)
def land(self):
self.drone.land()
def init_beacons(self):
self.beacons = []
self.beacons.append(Beacon('beacon1', np.array([0, 5, 0]), False, False))
self.beacons.append(Beacon('beacon2', np.array([5, 0, 0]), False, False))
self.beacons.append(Beacon('beacon3', np.array([0, -5, 0]), False, False))
self.beacons.append(Beacon('beacon4', np.array([-5, 0, 0]), False, False))
self.beacons.append(Beacon('beacon5', np.array([10, 0, 0]), False, False))
self.beacons.append(Beacon('initial', np.array([0, 0, 0]), False, False))
def get_next_beacon(self):
for beacon in self.beacons:
if beacon.is_reached() == False:
return beacon
return None