def __init__(self, sub):
self.sub = sub
self.print_info = FprintFactory(title=MISSION).fprint
self.print_bad = FprintFactory(title=MISSION, msg_color="red").fprint
self.print_good = FprintFactory(title=MISSION,
msg_color="green").fprint
self.buoys = {'red': Buoy(), 'green': Buoy(), 'yellow': Buoy()}
self.pattern_done = False
self.generate_pattern()
def __init__(self, sub):
self.sub = sub
self.print_info = FprintFactory(title=MISSION).fprint
self.print_bad = FprintFactory(title=MISSION, msg_color="red").fprint
self.print_good = FprintFactory(title=MISSION,
msg_color="green").fprint
self.tf_listener = tf.TransformListener()
# B = bottom; T = Top; L = left; R = right; C = center; O = unblocked;
# X = blocked;
self.targets = {'Wheel': Target()}
self.pattern_done = False
self.done = False
self.ltime = None
self.generate_pattern()
def run(sub):
fprint = FprintFactory(title='ACUTATOR TEST').fprint
@util.cancellableInlineCallbacks
def test():
yield sub.actuators.gripper_open()
yield sub.actuators.gripper_close()
yield sub.actuators.shoot_torpedo1()
yield sub.actuators.shoot_torpedo2()
yield sub.actuators.drop_marker()
yield sub.actuators.set('gripper', False)
yield sub.actuators.open('torpedo1')
yield sub.actuators.set_raw('torpedo1', True)
def err(err):
fprint('Error with actuators: {}'.format(err), msg_color='red')
global failed
failed = True
test_runner = test()
test_runner.addErrback(err)
yield test_runner
global failed
if not failed:
fprint('Success with all actuators!', msg_color='blue')
else:
fprint('Failed 1 or more actuator calls', msg_color='red')
def __init__(self, sub):
self.sub = sub
self.print_info = FprintFactory(title=MISSION).fprint
self.print_bad = FprintFactory(title=MISSION, msg_color="red").fprint
self.print_good = FprintFactory(title=MISSION,
msg_color="green").fprint
self.pattern_done = False
self.done = False
self.generate_pattern()
# self.get_path_marker = self.sub.nh.subscribe(
# '/vision/path_roi', RegionOfInterest)
self.get_direction = self.sub.nh.subscribe('/vision/path_direction',
String)
self.get_orange = self.sub.nh.subscribe('/vision/path_orange', String)
self.t = None
self.reset = True
self.starting_pos = sub.tx_pose()
def __init__(self, sub):
self.sub = sub
self.print_info = FprintFactory(title=MISSION).fprint
self.print_bad = FprintFactory(title=MISSION, msg_color="red").fprint
self.print_good = FprintFactory(
title=MISSION, msg_color="green").fprint
# B = bottom; T = Top; L = left; R = right; C = center; O = unblocked;
# X = blocked;
self.targets = {
'TCX': Target(),
'TRX': Target(),
'TLX': Target(),
'BCO': Target()
}
self.pattern_done = False
self.done = False
self.generate_pattern()
def __init__(self):
# Pull constants from config file
self.min_trans = rospy.get_param('~min_trans', .25)
self.max_velocity = rospy.get_param('~max_velocity', 1)
self.min_observations = rospy.get_param('~min_observations', 30)
self.camera = rospy.get_param('~camera_topic',
'/camera/front/left/image_rect_color')
# Instantiate remaining variables and objects
self._observations = deque()
self._pose_pairs = deque()
self._times = deque()
self._observations1 = deque()
self._pose_pairs1 = deque()
self._times1 = deque()
self._observations2 = deque()
self._pose_pairs2 = deque()
self._times2 = deque()
self.last_image_time = None
self.last_image = None
self.tf_listener = tf.TransformListener()
self.status = ''
self.est = None
self.est1 = None
self.est2 = None
self.plane = None
self.visual_id = 0
self.enabled = False
self.bridge = CvBridge()
self.print_info = FprintFactory(title=MISSION).fprint
# Image Subscriber and Camera Information
self.point_sub = rospy.Subscriber(
'/roi_pub', RegionOfInterest, self.acquire_targets)
self.image_sub = Image_Subscriber(self.camera, self.image_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = 'front_left_cam_optical'
self.markerPub0 = rospy.Publisher('estMarker0', Marker, queue_size=1)
self.markerPub1 = rospy.Publisher('estMarker1', Marker, queue_size=1)
self.markerPub2 = rospy.Publisher('estMarker2', Marker, queue_size=1)
# Ros Services so mission can be toggled and info requested
rospy.Service('~enable', SetBool, self.toggle_search)
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~pose', VisionRequest, self.request_board3d)
# Debug
self.debug = rospy.get_param('~debug', True)
def run(self, args):
self.vision_proxies.xyz_points.start()
global SPEED
global pub_cam_ray
fprint('Enabling cam_ray publisher')
pub_cam_ray = yield self.nh.advertise('/vamp/cam_ray', Marker)
yield self.nh.sleep(1)
fprint('Connecting camera')
cam_info_sub = yield self.nh.subscribe(
'/camera/front/left/camera_info', CameraInfo)
fprint('Obtaining cam info message')
cam_info = yield cam_info_sub.get_next_message()
model = PinholeCameraModel()
model.fromCameraInfo(cam_info)
# enable_service = self.nh.get_service_client("/vamp/enable", SetBool)
# yield enable_service(SetBoolRequest(data=True))
# try:
# vamp_txros = yield self.nh.get_service_client('/guess_location',
# GuessRequest)
# vamp_req = yield vamp_txros(GuessRequestRequest(item='vampire_slayer'))
# if vamp_req.found is False:
# use_prediction = False
# fprint(
# 'Forgot to add vampires to guess?',
# msg_color='yellow')
# else:
# fprint('Found vampires.', msg_color='green')
# yield self.move.set_position(mil_ros_tools.rosmsg_to_numpy(vamp_req.location.pose.position)).depth(0.5).go(speed=0.4)
# except Exception as e:
# fprint(e)
markers = MarkerArray()
pub_markers = yield self.nh.advertise('/torpedo/rays', MarkerArray)
pub_markers.publish(markers)
'''
Move Pattern
'''
yield self.move.left(1).go()
yield self.nh.sleep(2)
yield self.move.right(2).go()
yield self.nh.sleep(2)
yield self.move.down(.5).go()
yield self.nh.sleep(2)
yield self.move.up(.5).go()
yield self.move.forward(.75).go()
yield self.nh.sleep(2)
yield self.move.right(.75).go()
yield self.nh.sleep(2)
yield self.move.backward(.75).go()
yield self.move.left(1).go()
'''
Did we find something?
'''
res = yield self.vision_proxies.xyz_points.get_pose(target='buoy')
MISSION = 'Vampires'
print_info = FprintFactory(title=MISSION).fprint
if res.found:
print_info("CHARGING BUOY")
target_pose = rosmsg_to_numpy(res.pose.pose.position)
target_normal = rosmsg_to_numpy(res.pose.pose.orientation)[:2]
print('Normal: ', target_normal)
yield self.move.go(blind=True, speed=0.1) # Station hold
transform = yield self._tf_listener.get_transform(
'/map', '/base_link')
target_position = target_pose
# target_position = target_pose / target_normal
sub_pos = yield self.tx_pose()
print('Current Sub Position: ', sub_pos)
print('Map Position: ', target_position)
#sub_pos = transform._q_mat.dot(sub_pos[0] - transform._p)
#target_position = target_position - sub_pos[0]
# yield self.move.look_at_without_pitching(target_position).go(blind=True, speed=.25)
#yield self.move.relative(np.array([0, target_position[1], 0])).go(blind=True, speed=.1)
# Don't hit buoy yet
print("MOVING TO X: ", target_position[0])
print("MOVING TO Y: ", target_position[1])
yield self.move.set_position(
np.array([
target_position[0], target_position[1], target_position[2]
])).go(blind=True, speed=.1)
# Go behind it
#print('Going behind target')
#yield self.move.right(4).go(speed=1)
#yield self.move.forward(4).go(speed=1)
#yield self.move.left(4).go(speed=1)
# Hit buoy
#print('Hitting Target')
#yield self.move.strafe_backward(Y_OFFSET).go(speed=1)
print_info("Slaying the Vampire, good job Inquisitor.")
sub_pos = yield self.tx_pose()
print('Current Sub Position: ', sub_pos)
marker = Marker(ns='buoy',
action=visualization_msgs.Marker.ADD,
type=Marker.ARROW,
scale=Vector3(0.2, 0.5, 0),
points=np.array(
[Point(0, 0, 0), res.pose.pose.position]))
marker.id = 3
marker.header.frame_id = '/base_link'
marker.color.r = 1
marker.color.g = 0
marker.color.a = 1
markers.markers.append(marker)
pub_markers.publish(markers)
yield self.nh.sleep(0.5) # Throttle service calls
# print_info(info)
self.vision_proxies.xyz_points.stop()