def __init__(self, size=(640, 480)):
rospy.Subscriber("/ardrone/image_raw",
Image,
self.bg_callback,
queue_size=1)
self._start_screen(size)
pos_cam = [-1.5, -4, 4]
rot_cam = [-0, 0, 0, 1]
pos_drone = [-1.5, -1, 4]
rot_drone = [-0.3, 0, 0, 1]
self.pose_cam_callback(Pose.generate_stamped(pos_cam, rot_cam))
self.pose_drone_callback(Pose.generate_stamped(pos_drone, rot_drone))
def pose_callback(self, pose_stamped):
"""
Update `pose`, and select the best past image.
Short circuits if an evaluation function is still being run.
Parameters
----------
pose_stamped : PoseStamped
A pose message.
"""
rospy.logdebug("New pose")
self._pose_stamped = pose_stamped
self.pose = Pose(pose_stamped)
if self.evaluator.is_busy:
return
best_frame = self.evaluator.select_best_frame()
if best_frame is not None:
self.current_frame = best_frame
if not self.debug:
self.past_image_pub.publish(best_frame.image)
self.past_pose_pub.publish(best_frame.pose_stamped)
def test_offline(size=(640, 480)):
screen = Screen(size, model=Drone(), fov_diagonal=92)
threading.Thread(target=screen.run).start()
pos_cam = [1, 0, 0]
rot_cam = [0, 0, 0, 1]
pos_drone = [0, -3, 0]
rot_drone = [0, 0, 0, 1]
# pos_cam = [-0.5700, 0.08365, 0.0837]
# rot_cam = [0.0006, 0.0042, 0.0166, 0.9999]
# pos_drone = [-0.4767, 1.3597, 0.0770]
# rot_drone = [0.0078, 0.0087, 0.0059, 0.9999]
screen.pose_cam = Pose.generate_stamped(pos_cam, rot_cam)
screen.pose_drone = Pose.generate_stamped(pos_drone, rot_drone)
time.sleep(3)
screen.add_textures("media/bird.jpg")
def generate_random_pose(sequence=0):
"""
Generate a random pose.
Parameters
----------
sequence : Optional[int]
The sequence of the pose. Default is zero.
Returns
-------
The generated pose.
"""
return Pose.generate_stamped(position=np.random.rand(1, 3) / 100,
orientation=(0.1, 0, 0.1, 0.1),
sequence=sequence)
def generate_sine_pose(self, sequence=0):
"""
Generate a sinusoidal pose.
Parameters
----------
sequence : Optional[int]
The sequence of the pose. Default is zero.
Returns
-------
The generated pose.
"""
idx = sequence % self.n_items
return Pose.generate_stamped(position=(30 * self.x_pos[idx], 0, 0),
orientation=(0, 1, 0, -0.6),
sequence=sequence)
def pose_drone_callback(self, pose_drone):
self.screen.pose_drone = Pose(pose_drone)
def pose_cam_callback(self, pose_cam):
self.screen.pose_cam = Pose(pose_cam)