def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = mil_ros_tools.rosmsg_to_numpy(msg.linear)
angular = mil_ros_tools.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(self._position_gain * linear)
self.diff_position = np.subtract(self.cur_position, self.target_position)
gained_angular = self._orientation_gain * angular
skewed = mil_ros_tools.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
self.target_distance = round(np.linalg.norm(np.array([self.diff_position[0], self.diff_position[1]])), 3)
self.target_depth = round(self.diff_position[2], 3)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank)
self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
def run(self, args):
fprint('Getting Guess Locations')
sub_start_position, sub_start_orientation = yield self.tx_pose()
gate_txros = yield self.nh.get_service_client('/guess_location',
GuessRequest)
gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1'))
gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2'))
gate_1 = mil_ros_tools.rosmsg_to_numpy(
gate_1_req.location.pose.position)
gate_2 = mil_ros_tools.rosmsg_to_numpy(
gate_2_req.location.pose.position)
mid = (gate_1 + gate_2) / 2
fprint('Found mid {}'.format(mid))
fprint('Looking at gate')
yield self.move.down(DOWN).set_orientation(sub_start_orientation).go(
speed=DOWN_SPEED)
yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED)
fprint('Going!')
yield self.move.set_position(mid).depth(DOWN).go(speed=SPEED)
def set_pose_server(self, srv):
'''Set the pose of the submarine
TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id
'''
rospy.logwarn("Manually setting position of simulated vehicle")
position = mil_ros_tools.rosmsg_to_numpy(srv.pose.position)
self.body.setPosition(position)
rotation_q = mil_ros_tools.rosmsg_to_numpy(srv.pose.orientation)
rotation_norm = np.linalg.norm(rotation_q)
velocity = mil_ros_tools.rosmsg_to_numpy(srv.twist.linear)
angular = mil_ros_tools.rosmsg_to_numpy(srv.twist.angular)
self.body.setLinearVel(velocity)
self.body.setAngularVel(angular)
# If the commanded rotation is valid
if np.isclose(rotation_norm, 1., atol=1e-2):
self.body.setQuaternion(mil_ros_tools.normalize(rotation_q))
else:
rospy.logwarn(
"Commanded quaternion was not a unit quaternion, NOT setting rotation"
)
return SimSetPoseResponse()
def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = mil_ros_tools.rosmsg_to_numpy(msg.linear)
angular = mil_ros_tools.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(
self._position_gain * linear)
self.diff_position = np.subtract(self.cur_position,
self.target_position)
gained_angular = self._orientation_gain * angular
skewed = mil_ros_tools.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
self.target_distance = round(
np.linalg.norm(
np.array([self.diff_position[0], self.diff_position[1]])), 3)
self.target_depth = round(self.diff_position[2], 3)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(
blank)
self.publish_target_pose(self.target_position,
self.target_orientation_quaternion)
def run(self, args):
fprint('Getting Guess Locations')
sub_start_position, sub_start_orientation = yield self.tx_pose()
gate_txros = yield self.nh.get_service_client('/guess_location',
GuessRequest)
gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1'))
gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2'))
gate_1 = mil_ros_tools.rosmsg_to_numpy(
gate_1_req.location.pose.position)
gate_2 = mil_ros_tools.rosmsg_to_numpy(
gate_2_req.location.pose.position)
#mid = (gate_1 + gate_2) / 2
#mid = gate_1
fprint('Found mid {}'.format(gate_1))
fprint('Looking at gate')
# yield self.move.down(DOWN).set_orientation(sub_start_orientation).go(
#########speed=DOWN_SPEED)
# yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED)
fprint('Going!')
yield self.move.set_position(gate_1).depth(DOWN).go(speed=SPEED)
yield self.move.forward(1).go(speed=SPEED)
def run(sub):
print_info = text_effects.FprintFactory(title=MISSION).fprint
print_bad = text_effects.FprintFactory(title=MISSION, msg_color="red").fprint
print_good = text_effects.FprintFactory(title=MISSION, msg_color="green").fprint
print_info("STARTING")
# Set geometry of marker model
print_info("SETTING GEOMETRY")
polygon = RectFinder(LENGTH, WIDTH).to_polygon()
yield sub.vision_proxies.orange_rectangle.set_geometry(polygon)
# Wait for vision services, enable perception
print_info("ACTIVATING PERCEPTION SERVICE")
yield sub.vision_proxies.orange_rectangle.start()
pattern = []
if DO_PATTERN:
start = sub.move.zero_roll_and_pitch()
r = SEARCH_RADIUS_METERS
pattern = [start.zero_roll_and_pitch(),
start.right(r),
start.forward(r),
start.left(r),
start.backward(r),
start.right(2 * r),
start.forward(2 * r),
start.left(2 * r),
start.backward(2 * r)]
s = Searcher(sub, sub.vision_proxies.orange_rectangle.get_pose, pattern)
resp = None
print_info("RUNNING SEARCH PATTERN")
resp = yield s.start_search(loop=False, timeout=TIMEOUT_SECONDS, spotings_req=1)
if resp is None or not resp.found:
print_bad("MARKER NOT FOUND")
defer.returnValue(None)
print_good("PATH MARKER POSE FOUND")
assert(resp.pose.header.frame_id == "/map")
move = sub.move
position = rosmsg_to_numpy(resp.pose.pose.position)
position[2] = move._pose.position[2] # Leave Z alone!
orientation = rosmsg_to_numpy(resp.pose.pose.orientation)
move = move.set_position(position).set_orientation(orientation).zero_roll_and_pitch()
# Ensure SubjuGator continues to face same general direction as before (doesn't go in opposite direction)
odom_forward = tf.transformations.quaternion_matrix(sub.move._pose.orientation).dot(forward_vec)
marker_forward = tf.transformations.quaternion_matrix(orientation).dot(forward_vec)
if FACE_FORWARD and np.sign(odom_forward[0]) != np.sign(marker_forward[0]):
move = move.yaw_right(np.pi)
print_info("MOVING TO MARKER AT {}".format(move._pose.position))
yield move.go(speed=SPEED)
print_good("ALIGNED TO PATH MARKER. MOVE FORWARD TO NEXT CHALLENGE!")
yield sub.vision_proxies.orange_rectangle.stop()
defer.returnValue(True)
def test_make_wrench_stamped(self):
'''Test that wrenchmaking works'''
for k in range(10):
force = np.random.random(3) * 10
torque = np.random.random(3) * 10
wrench_msg = make_wrench_stamped(force, torque, frame='/enu')
msg_force = rosmsg_to_numpy(wrench_msg.wrench.force) # noqa
msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque) # noqa
self.assertIsNotNone(msg_force)
self.assertIsNotNone(msg_torque)
def test_make_wrench_stamped(self):
'''Test that wrenchmaking works'''
for k in range(10):
force = np.random.random(3) * 10
torque = np.random.random(3) * 10
wrench_msg = make_wrench_stamped(force, torque, frame='/enu')
msg_force = rosmsg_to_numpy(wrench_msg.wrench.force) # noqa
msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque) # noqa
self.assertIsNotNone(msg_force)
self.assertIsNotNone(msg_torque)
def find_gate(self,
objects,
ray,
min_distance_away=2.85,
max_distance_away=3.3,
perp_threshold=0.5,
depth_threshold=1):
'''
find_gate: search for two objects that satisfy critria
Parameters:
ray: direction we expect gate to be near
min_distance_away: minimum distance for the two poles
max_distance_away: max distance the two objects can be away from each other
perp_threshold: max dot product value for perpindicular test with ray
depth_threshold: make sure the two objects have close enough depth
'''
for o in objects:
p = rosmsg_to_numpy(o.pose.position)
for o2 in objects:
if o2 is o:
continue
p2 = rosmsg_to_numpy(o2.pose.position)
if distance.euclidean(p, p2) > max_distance_away:
fprint('Poles too far away. Distance {}'.format(
distance.euclidean(p, p2)))
continue
if distance.euclidean(p, p2) < min_distance_away:
fprint('Poles too close. Distance {}'.format(
distance.euclidean(p, p2)))
continue
line = p - p2
perp = line.dot(ray)
perp = perp / np.linalg.norm(perp)
if not (-perp_threshold <= perp <= perp_threshold):
fprint('Not perpendicular. Dot {}'.format(perp))
pass
# continue
print('Dist {}'.format(line))
if abs(line[2] > depth_threshold):
print('Not similar height. Height: {}. Thresh: '.format(
line[2], depth_threshold))
continue
if abs(line[0]) < 1 and abs(line[1]) < 1:
fprint('Objects on top of one another. x {}, y {}'.format(
line[0], line[1]))
continue
return (p, p2)
return None
def twist_cb(self, msg):
linear = mil_ros_tools.rosmsg_to_numpy(msg.linear)
angular = mil_ros_tools.rosmsg_to_numpy(msg.angular)
if self.behavior == 'wrench':
# Directly transcribe linear and angular 'velocities' to torque
# TODO: Setup actual TF!
self.wrench_pub.publish(
mil_ros_tools.make_wrench_stamped(
force=self.linear_gain * self.world_to_body.dot(linear),
torque=self.angular_gain *
self.world_to_body.dot(angular)))
else:
raise NotImplementedError
def test_rosmsg_to_numpy_quaternion(self):
'''Test a rosmsg conversion for a geometry_msgs/Quaternion'''
# I know this is not a unit quaternion
q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2)
numpy_array = rosmsg_to_numpy(q)
np.testing.assert_allclose(np.array([0.7, 0.7, 0.1, 0.2]), numpy_array)
def transform_to_baselink(self, sub, pinger_1_req, pinger_2_req):
transform = yield sub._tf_listener.get_transform('/base_link', '/map')
position = yield sub.pose.position
pinger_guess = [
transform._q_mat.dot(
mil_ros_tools.rosmsg_to_numpy(x.location.pose.position) -
position) for x in (pinger_1_req, pinger_2_req)
]
for idx, guess in enumerate(pinger_guess):
marker = Marker(ns='pinger',
action=visualization_msgs.Marker.ADD,
type=Marker.ARROW,
scale=Vector3(0.1, 0.2, 0),
points=np.array([
Point(0, 0, 0),
Point(guess[0], guess[1], guess[2])
]))
marker.id = idx
marker.header.frame_id = '/base_link'
marker.color.r = 0
marker.color.g = 1
marker.color.a = 1
global markers
markers.markers.append(marker)
defer.returnValue(pinger_guess)
def twist_cb(self, msg):
linear = mil_ros_tools.rosmsg_to_numpy(msg.linear)
angular = mil_ros_tools.rosmsg_to_numpy(msg.angular)
if self.behavior == 'wrench':
# Directly transcribe linear and angular 'velocities' to torque
# TODO: Setup actual TF!
self.wrench_pub.publish(
mil_ros_tools.make_wrench_stamped(
force=self.linear_gain * self.world_to_body.dot(linear),
torque=self.angular_gain * self.world_to_body.dot(angular)
)
)
else:
raise NotImplementedError
def test_rosmsg_to_numpy_custom(self):
'''Test a rosmsg conversion for a custom msg'''
pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14)
numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta'])
np.testing.assert_allclose(
np.array([1.0, 2.0, 3.14]),
numpy_array
)
def test_rosmsg_to_numpy_vector(self):
'''Test a rosmsg conversion for a geometry_msgs/Vector'''
v = Vector3(x=0.1, y=99., z=21.)
numpy_array = rosmsg_to_numpy(v)
np.testing.assert_allclose(
np.array([0.1, 99., 21.]),
numpy_array
)
def run(sub):
yield sub.vision_proxies.start_gate.start()
# Add search pattern if needed...
search_pattern = []
search = Searcher(sub, sub.vision_proxies.start_gate.get_pose,
search_pattern)
resp = None
fprint('Searching...')
resp = yield search.start_search(loop=False, timeout=100, spotings_req=1)
if resp is None or not resp.found:
fprint("No gate found...", msg_color="red")
defer.returnValue(None)
position = rosmsg_to_numpy(resp.pose.pose.position)
orientation = rosmsg_to_numpy(resp.pose.pose.orientation)
fprint('Gate\'s position in map is: {}'.format(position))
fprint('Gate\'s orientation in map is: {}'.format(orientation))
# Get the normal vector, which is assumed to be the [1,0,0] unit vector
normal = tf.transformations.quaternion_matrix(orientation).dot(
np.array([1, 0, 0, 0]))[0:3]
fprint('Computed normal vector: {}'.format(normal))
# Computer points before and after the gate for the sub to go to
point_before = position + FACTOR_DISTANCE_BEFORE * normal
point_after = position - FACTOR_DISTANCE_AFTER * normal
# go in front of gate
fprint('Moving infront of gate {}'.format(point_before))
yield sub.move.set_position(point_before).look_at(
point_after).zero_roll_and_pitch().go(speed=SPEED)
# go through the gate
fprint('YOLO! Going through gate {}'.format(point_after))
yield sub.move.set_position(point_after).zero_roll_and_pitch().go(
speed=SPEED)
yield sub.vision_proxies.start_gate.stop()
defer.returnValue(True)
def request_wrench_cb(self, msg):
'''Callback for requesting a wrench'''
time_now = rospy.Time.now()
if (time_now - self.last_command_time) < self._min_command_time:
return
else:
self.last_command_time = rospy.Time.now()
force = rosmsg_to_numpy(msg.wrench.force)
torque = rosmsg_to_numpy(msg.wrench.torque)
wrench = np.hstack([force, torque])
success = False
while not success:
u, success = self.map(wrench)
if not success:
# If we fail to compute, shrink the wrench
wrench = wrench * 0.75
continue
thrust_cmds = []
# Assemble the list of thrust commands to send
for name, thrust in zip(self.thruster_name_map, u):
# > Can speed this up by avoiding appends
if name in self.dropped_thrusters:
thrust = 0 # Sending a command packet is an opportunity to detect thruster recovery
# Simulate thruster deadband
if np.fabs(thrust) < self.min_commandable_thrust:
thrust = 0
thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
actual_wrench = self.B.dot(u)
self.actual_wrench_pub.publish(
msg_helpers.make_wrench_stamped(actual_wrench[:3],
actual_wrench[3:],
frame='/base_link'))
mapper_wrench_error = wrench - actual_wrench
self.wrench_error_pub.publish(
msg_helpers.make_wrench_stamped(mapper_wrench_error[:3],
mapper_wrench_error[3:],
frame='/base_link'))
self.thruster_pub.publish(thrust_cmds)
def run(sub):
yield sub.vision_proxies.start_gate.start()
# Add search pattern if needed...
search_pattern = []
search = Searcher(
sub,
sub.vision_proxies.start_gate.get_pose,
search_pattern)
resp = None
fprint('Searching...')
resp = yield search.start_search(loop=False, timeout=100, spotings_req=1)
if resp is None or not resp.found:
fprint("No gate found...", msg_color="red")
defer.returnValue(None)
position = rosmsg_to_numpy(resp.pose.pose.position)
orientation = rosmsg_to_numpy(resp.pose.pose.orientation)
fprint('Gate\'s position in map is: {}'.format(position))
fprint('Gate\'s orientation in map is: {}'.format(orientation))
# Get the normal vector, which is assumed to be the [1,0,0] unit vector
normal = tf.transformations.quaternion_matrix(
orientation).dot(np.array([1, 0, 0, 0]))[0:3]
fprint('Computed normal vector: {}'.format(normal))
# Computer points before and after the gate for the sub to go to
point_before = position + FACTOR_DISTANCE_BEFORE * normal
point_after = position - FACTOR_DISTANCE_AFTER * normal
# go in front of gate
fprint('Moving infront of gate {}'.format(point_before))
yield sub.move.set_position(point_before).look_at(point_after).zero_roll_and_pitch().go(speed=SPEED)
# go through the gate
fprint('YOLO! Going through gate {}'.format(point_after))
yield sub.move.set_position(point_after).zero_roll_and_pitch().go(speed=SPEED)
yield sub.vision_proxies.start_gate.stop()
defer.returnValue(True)
def run(sub):
print_info = text_effects.FprintFactory(title=MISSION)
print_bad = text_effects.FprintFactory(title=MISSION, msg_color="red")
print_good = text_effects.FprintFactory(title=MISSION, msg_color="green")
print_info.fprint("STARTING")
# Wait for vision services, enable perception
print_info.fprint("ACTIVATING PERCEPTION SERVICE")
sub.vision_proxies.path_marker.start()
pattern = []
#pattern = [sub.move.right(1), sub.move.forward(1), sub.move.left(1), sub.move.backward(1),
# sub.move.right(2), sub.move.forward(2), sub.move.left(2), sub.move.backward(2)]
s = Searcher(sub, sub.vision_proxies.path_marker.get_pose, pattern)
resp = None
print_info.fprint("RUNNING SEARCH PATTERN")
resp = yield s.start_search(loop=False, timeout=TIMEOUT_SECONDS)
if resp is None:
print_bad.fprint("MARKER NOT FOUND")
defer.returnValue(None)
print_good.fprint("PATH MARKER POSE FOUND")
print_info.fprint("TRANFORMING MARKER POSE TO /map FROM {}".format(
resp.pose.header.frame_id))
cam_to_baselink = yield sub._tf_listener.get_transform(
'/base_link', '/' + resp.pose.header.frame_id)
cam_to_map = yield sub._tf_listener.get_transform(
'/map', '/' + resp.pose.header.frame_id)
marker_position = cam_to_map.transform_point(
rosmsg_to_numpy(resp.pose.pose.position))
marker_orientation = cam_to_map.transform_quaternion(
rosmsg_to_numpy(resp.pose.pose.orientation))
move = sub.move.set_orientation(marker_orientation).zero_roll_and_pitch()
position = marker_position.copy()
position[2] = move._pose.position[2]
position[0] = position[0] + cam_to_baselink._p[0]
position[1] = position[1] + cam_to_baselink._p[1]
move = move.set_position(position)
print_info.fprint("MOVING TO MARKER POSE")
yield move.go(speed=0.2)
print_good.fprint("ALIGNED TO PATH MARKER, DONE!")
sub.vision_proxies.path_marker.stop()
defer.returnValue(True)
def test_rosmsg_to_numpy_quaternion(self):
'''Test a rosmsg conversion for a geometry_msgs/Quaternion'''
# I know this is not a unit quaternion
q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2)
numpy_array = rosmsg_to_numpy(q)
np.testing.assert_allclose(
np.array([0.7, 0.7, 0.1, 0.2]),
numpy_array
)
def search(self):
while True:
info = 'FOUND: '
for buoy in self.buoys:
res = yield self.sub.vision_proxies.buoy.get_pose(target=buoy)
if res.found:
self.buoys[buoy].update_position(rosmsg_to_numpy(res.pose.pose.position))
if self.buoys[buoy].position is not None:
info += buoy + ' '
yield self.sub.nh.sleep(0.5) # Throttle service calls
self.print_info(info)
def search(self):
global markers
markers = MarkerArray()
pub_markers = yield self.nh.advertise('/torpedo/rays', MarkerArray)
while True:
info = 'CURRENT TARGETS: '
target = 'BCO'
pub_markers.publish(markers)
'''
In the event we want to attempt other targets beyond bare minimum
for target in self.targets. Eventually we will want to take the
best target, which is the TCX target. Priority targetting will
come once we confirm we can actually unblock currently blocked
targets.
'''
print("REQUESTING POSE")
res = yield self.vision_proxies.xyz_points.get_pose(target='board')
if res.found:
print("POSE FOUND!")
self.ltime = res.pose.header.stamp
self.targets[target].update_position(
rosmsg_to_numpy(res.pose.pose.position))
self.normal = rosmsg_to_numpy(res.pose.pose.orientation)[:3]
marker = Marker(ns='torp_board',
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)
if self.targets[target].position is not None:
print("TARGET IS NOT NONE")
info += target + ' '
yield self.nh.sleep(0.1) # Throttle service calls
self.print_info(info)
def run(sub):
fprint('Getting Guess Locations')
sub_start_position, sub_start_orientation = yield sub.tx_pose()
gate_txros = yield sub.nh.get_service_client('/guess_location',
GuessRequest)
gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1'))
gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2'))
gate_1 = mil_ros_tools.rosmsg_to_numpy(gate_1_req.location.pose.position)
gate_2 = mil_ros_tools.rosmsg_to_numpy(gate_2_req.location.pose.position)
mid = (gate_1 + gate_2) / 2
yield sub.down(DOWN).set_orientation(sub_start_orientation).go(
speed=DOWN_SPEED)
yield sub.look_at_without_pitching(mid).go(speed=DOWN_SPEED)
yield sub.set_position(mid).depth(DOWN).go(speed=SPEED)
def sanity_check(self, coordinate, timestamp):
'''
Check if the observation is unreasonable. More can go here if we want.
'''
if self.last_odom is None:
return False
linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear)
if np.linalg.norm(linear_velocity) > self.max_velocity:
return False
return True
def sanity_check(self, coordinate, timestamp):
'''
Check if the observation is unreasonable. More can go here if we want.
'''
if self.last_odom is None:
return False
linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear)
if np.linalg.norm(linear_velocity) > self.max_velocity:
return False
return True
def search(self):
while True:
info = 'FOUND: '
for buoy in self.buoys:
res = yield self.sub.vision_proxies.buoy.get_pose(target=buoy)
if res.found:
self.buoys[buoy].update_position(
rosmsg_to_numpy(res.pose.pose.position))
if self.buoys[buoy].position is not None:
info += buoy + ' '
yield self.sub.nh.sleep(0.5) # Throttle service calls
self.print_info(info)
def update_object(self, object_msg, class_probabilities):
object_id = object_msg.id
# Update the total class probabilities
if object_id in self.object_map:
self.object_map[object_id] += class_probabilities
else:
self.object_map[object_id] = class_probabilities
total_probabilities = self.object_map[object_id]
# Guess the type of object based
most_likely_index = np.argmax(total_probabilities)
most_likely_name = self.classifier.CLASSES[most_likely_index]
# Unforuntely this doesn't really work'
if most_likely_name in self.BLACK_OBJECT_CLASSES:
object_scale = rosmsg_to_numpy(object_msg.scale)
object_volume = object_scale.dot(object_scale)
object_area = object_scale[:2].dot(object_scale[:2])
height = object_scale[2]
if object_id in self.volume_means:
self.volume_means[object_id].add_value(object_volume)
self.area_means[object_id].add_value(object_area)
else:
self.volume_means[object_id] = RunningMean(object_volume)
self.area_means[object_id] = RunningMean(object_area)
running_mean_volume = self.volume_means[object_id].mean
running_mean_area = self.area_means[object_id].mean
if height > self.TOTEM_MIN_HEIGHT:
black_guess = 'black_totem'
else:
black_guess_index = np.argmin(
np.abs(self.BLACK_OBJECT_VOLUMES - running_mean_volume))
black_guess = self.POSSIBLE_BLACK_OBJECTS[black_guess_index]
most_likely_name = black_guess
rospy.loginfo(
'{} current/running volume={}/{} area={}/{} height={}-> {}'.
format(object_id, object_volume, running_mean_volume,
object_area, running_mean_area, height, black_guess))
obj_title = object_msg.labeled_classification
probability = class_probabilities[most_likely_index]
rospy.loginfo('Object {} {} classified as {} ({}%)'.format(
object_id, object_msg.labeled_classification, most_likely_name,
probability * 100.))
if obj_title != most_likely_name:
cmd = '{}={}'.format(object_id, most_likely_name)
rospy.loginfo('Updating object {} to {}'.format(
object_id, most_likely_name))
if not self.is_training:
self.database_client(ObjectDBQueryRequest(cmd=cmd))
return most_likely_name
def request_wrench_cb(self, msg):
'''Callback for requesting a wrench'''
time_now = rospy.Time.now()
if (time_now - self.last_command_time) < self._min_command_time:
return
else:
self.last_command_time = rospy.Time.now()
force = rosmsg_to_numpy(msg.wrench.force)
torque = rosmsg_to_numpy(msg.wrench.torque)
wrench = np.hstack([force, torque])
success = False
while not success:
u, success = self.map(wrench)
if not success:
# If we fail to compute, shrink the wrench
wrench = wrench * 0.75
continue
thrust_cmds = []
# Assemble the list of thrust commands to send
for name, thrust in zip(self.thruster_name_map, u):
# > Can speed this up by avoiding appends
if name in self.dropped_thrusters:
continue # Ignore dropped thrusters
if np.fabs(thrust) < self.min_commandable_thrust:
thrust = 0
thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
# TODO: Make this account for dropped thrusters
actual_wrench = self.B.dot(u)
self.actual_wrench_pub.publish(
msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:])
)
self.thruster_pub.publish(thrust_cmds)
def request_wrench_cb(self, msg):
'''Callback for requesting a wrench'''
time_now = rospy.Time.now()
if (time_now - self.last_command_time) < self._min_command_time:
return
else:
self.last_command_time = rospy.Time.now()
force = rosmsg_to_numpy(msg.wrench.force)
torque = rosmsg_to_numpy(msg.wrench.torque)
wrench = np.hstack([force, torque])
success = False
while not success:
u, success = self.map(wrench)
if not success:
# If we fail to compute, shrink the wrench
wrench = wrench * 0.75
continue
thrust_cmds = []
# Assemble the list of thrust commands to send
for name, thrust in zip(self.thruster_name_map, u):
# > Can speed this up by avoiding appends
if name in self.dropped_thrusters:
continue # Ignore dropped thrusters
# Simulate thruster deadband
if np.fabs(thrust) < self.min_commandable_thrust:
thrust = 0
thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
actual_wrench = self.B.dot(u)
self.actual_wrench_pub.publish(
msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:])
)
self.thruster_pub.publish(thrust_cmds)
def set_pose_server(self, srv):
'''Set the pose of the submarine
TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id
'''
rospy.logwarn("Manually setting position of simulated vehicle")
position = mil_ros_tools.rosmsg_to_numpy(srv.pose.position)
self.body.setPosition(position)
rotation_q = mil_ros_tools.rosmsg_to_numpy(srv.pose.orientation)
rotation_norm = np.linalg.norm(rotation_q)
velocity = mil_ros_tools.rosmsg_to_numpy(srv.twist.linear)
angular = mil_ros_tools.rosmsg_to_numpy(srv.twist.angular)
self.body.setLinearVel(velocity)
self.body.setAngularVel(angular)
# If the commanded rotation is valid
if np.isclose(rotation_norm, 1., atol=1e-2):
self.body.setQuaternion(mil_ros_tools.normalize(rotation_q))
else:
rospy.logwarn("Commanded quaternion was not a unit quaternion, NOT setting rotation")
return SimSetPoseResponse()
def check_continuity(self, odom):
'''
On new odom message, find distance in position between this message and the last one.
If the distance is > MAX_JUMP, raise the alarm
'''
position = rosmsg_to_numpy(odom.pose.pose.position)
if self.last_position is not None:
jump = np.linalg.norm(position - self.last_position)
if jump > self.MAX_JUMP and not self._killed:
self._raised = True # Avoid raising multiple times
rospy.logwarn('ODOM DISCONTINUITY DETECTED')
self.ab.raise_alarm(problem_description='ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format(jump),
severity=5)
self.last_position = position
self.last_time = odom.header.stamp
def _sort_by_angle(objects, ray, start_point):
"""
_sort_by_angle: returns object list sorted by angle
Parameters:
objects:
ray: directional unit vector
start_point: base point for vector in map
"""
positions = [
mil_ros_tools.rosmsg_to_numpy(o.pose.position) for o in objects
]
dots = [(p / np.linalg.norm(p) - start_point).dot(ray)
for p in positions]
idx = np.argsort(dots)
return np.array(objects)[idx]
def _sort_by_angle(objects, ray, start_point):
"""
_sort_by_angle: returns object list sorted by angle
Parameters:
objects:
ray: directional unit vector
start_point: base point for vector in map
"""
positions = [
mil_ros_tools.rosmsg_to_numpy(o.pose.position) for o in objects
]
dots = [(p / np.linalg.norm(p) - start_point).dot(ray)
for p in positions]
idx = np.argsort(dots)
return np.array(objects)[idx]
def check_continuity(self, odom):
'''
On new odom message, find distance in position between this message and the last one.
If the distance is > MAX_JUMP, raise the alarm
'''
position = rosmsg_to_numpy(odom.pose.pose.position)
if self.last_position is not None:
jump = np.linalg.norm(position - self.last_position)
if jump > self.MAX_JUMP and not self._killed:
self._raised = True # Avoid raising multiple times
rospy.logwarn('ODOM DISCONTINUITY DETECTED')
self.ab.raise_alarm(
problem_description=
'ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format(
jump),
severity=5)
self.last_position = position
self.last_time = odom.header.stamp
def _get_objects_within_cone(objects, start_point, ray, angle_tol,
distance_tol):
ray = ray / np.linalg.norm(ray)
out = []
for o in objects:
print '=' * 50
pos = mil_ros_tools.rosmsg_to_numpy(o.pose.position)
print 'pos {}'.format(pos)
dist = np.dot(pos - start_point, ray)
print 'dist {}'.format(dist)
if dist > distance_tol or dist < 0:
continue
vec_for_pos = pos - start_point
vec_for_pos = vec_for_pos / np.linalg.norm(vec_for_pos)
angle = np.arccos(vec_for_pos.dot(ray)) * 180 / np.pi
print 'angle {}'.format(angle)
if angle > angle_tol:
continue
out.append(o)
return out
def _get_objects_within_cone(objects, start_point, ray, angle_tol,
distance_tol):
ray = ray / np.linalg.norm(ray)
out = []
for o in objects:
print '=' * 50
pos = mil_ros_tools.rosmsg_to_numpy(o.pose.position)
print 'pos {}'.format(pos)
dist = np.dot(pos - start_point, ray)
print 'dist {}'.format(dist)
if dist > distance_tol or dist < 0:
continue
vec_for_pos = pos - start_point
vec_for_pos = vec_for_pos / np.linalg.norm(vec_for_pos)
angle = np.arccos(vec_for_pos.dot(ray)) * 180 / np.pi
print 'angle {}'.format(angle)
if angle > angle_tol:
continue
out.append(o)
return out
def search(self):
while True:
info = 'CURRENT TARGETS: '
target = 'BCO'
'''
In the event we want to attempt other targets beyond bare minimum
for target in self.targets. Eventually we will want to take the
best target, which is the TCX target. Priority targetting will
come once we confirm we can actually unblock currently blocked
targets.
'''
res = yield self.sub.vision_proxies.arm_torpedos.get_pose(
target='board')
if res.found:
self.targets[target].update_position(
rosmsg_to_numpy(res.pose.pose.position))
if self.targets[target].position is not None:
info += target + ' '
yield self.sub.nh.sleep(0.5) # Throttle service calls
self.print_info(info)
def search(self):
global markers
markers = MarkerArray()
pub_markers = yield self.sub.nh.advertise('/roulette_wheel/rays', MarkerArray)
while True:
info = 'CURRENT TARGETS: '
target = 'Wheel'
pub_markers.publish(markers)
'''
In the event we want to attempt other targets beyond bare minimum
for target in self.targets. Eventually we will want to take the
best target, which is the TCX target. Priority targetting will
come once we confirm we can actually unblock currently blocked
targets.
'''
res = yield self.sub.vision_proxies.arm_torpedos.get_pose(
target='roulette_wheel')
if res.found:
self.ltime = res.pose.header.stamp
self.targets[target].update_position(
rosmsg_to_numpy(res.pose.pose.position))
marker = Marker(
ns='roulette_wheel',
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)
if self.targets[target].position is not None:
info += target + ' '
yield self.sub.nh.sleep(0.5) # Throttle service calls
self.print_info(info)
def transform_to_baselink(sub, pinger_1_req, pinger_2_req):
transform = yield sub._tf_listener.get_transform('/base_link', '/map')
pinger_guess = [
transform._q_mat.dot(
mil_ros_tools.rosmsg_to_numpy(x.location.pose.position) -
transform._p) for x in (pinger_1_req, pinger_2_req)
]
for idx, guess in enumerate(pinger_guess):
marker = Marker(
ns='pinger',
action=visualization_msgs.Marker.ADD,
type=Marker.ARROW,
scale=Vector3(0.1, 0.2, 0),
points=np.array(
[Point(0, 0, 0),
Point(guess[0], guess[1], guess[2])]))
marker.id = idx
marker.header.frame_id = '/base_link'
marker.color.r = 0
marker.color.g = 1
marker.color.a = 1
global markers
markers.markers.append(marker)
defer.returnValue(pinger_guess)
def align_to_board(msg, sub):
position = rosmsg_to_numpy(msg.position)
yield sub.move.set_position(position).zero_roll_and_pitch().go()
def test_rosmsg_to_numpy_vector(self):
'''Test a rosmsg conversion for a geometry_msgs/Vector'''
v = Vector3(x=0.1, y=99., z=21.)
numpy_array = rosmsg_to_numpy(v)
np.testing.assert_allclose(np.array([0.1, 99., 21.]), numpy_array)
def from_Pose(cls, frame_id, msg):
return cls(frame_id, rosmsg_to_numpy(msg.position), rosmsg_to_numpy(msg.orientation))
def align_to_board(msg, sub):
position = rosmsg_to_numpy(msg.position)
yield sub.move.set_position(position).zero_roll_and_pitch().go()
def run(sub):
global SPEED
global pub_cam_ray
fprint('Enabling cam_ray publisher')
pub_cam_ray = yield sub.nh.advertise('/dice/cam_ray', Marker)
yield sub.nh.sleep(1)
fprint('Connecting camera')
cam_info_sub = yield sub.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 = sub.nh.get_service_client("/dice/enable", SetBool)
yield enable_service(SetBoolRequest(data=True))
dice_sub = yield sub.nh.subscribe('/dice/points', Point)
found = {}
history_tf = {}
while len(found) != 2:
fprint('Getting dice xy')
dice_xy = yield dice_sub.get_next_message()
found[dice_xy.z] = mil_ros_tools.rosmsg_to_numpy(dice_xy)[:2]
fprint(found)
out = yield get_transform(sub, model, found[dice_xy.z])
history_tf[dice_xy.z] = out
if len(found) > 1:
tmp = found.values()[0] - found.values()[1]
# Make sure the two points are at least 100 pixels off
diff = np.hypot(tmp[0], tmp[1])
fprint('Distance between buoys {}'.format(diff))
if diff < 40:
found = {}
yield enable_service(SetBoolRequest(data=False))
start = sub.move.zero_roll_and_pitch()
yield start.go()
for i in range(2):
fprint('Hitting dice {}'.format(i))
# Get one of the dice
dice, xy = found.popitem()
fprint('dice {}'.format(dice))
ray, base = history_tf[dice]
where = base + 3 * ray
fprint(where)
fprint('Moving!', msg_color='yellow')
fprint('Current position: {}'.format(sub.pose.position))
fprint('zrp')
yield sub.move.zero_roll_and_pitch().go(blind=True)
yield sub.nh.sleep(4)
fprint('hitting', msg_color='yellow')
yield sub.move.look_at(where).go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
yield sub.move.set_position(where).go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
fprint('going back', msg_color='yellow')
yield start.go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
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()
def run(sub):
global SPEED
global pub_cam_ray
fprint('Enabling cam_ray publisher')
pub_cam_ray = yield sub.nh.advertise('/dice/cam_ray', Marker)
yield sub.nh.sleep(1)
fprint('Connecting camera')
cam_info_sub = yield sub.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 = sub.nh.get_service_client("/dice/enable", SetBool)
yield enable_service(SetBoolRequest(data=True))
dice_sub = yield sub.nh.subscribe('/dice/points', Point)
found = {}
history_tf = {}
while len(found) != 2:
fprint('Getting dice xy')
dice_xy = yield dice_sub.get_next_message()
found[dice_xy.z] = mil_ros_tools.rosmsg_to_numpy(dice_xy)[:2]
fprint(found)
out = yield get_transform(sub, model, found[dice_xy.z])
history_tf[dice_xy.z] = out
if len(found) > 1:
tmp = found.values()[0] - found.values()[1]
# Make sure the two points are at least 100 pixels off
diff = np.hypot(tmp[0], tmp[1])
fprint('Distance between buoys {}'.format(diff))
if diff < 40:
found = {}
yield enable_service(SetBoolRequest(data=False))
start = sub.move.zero_roll_and_pitch()
yield start.go()
for i in range(2):
fprint('Hitting dice {}'.format(i))
# Get one of the dice
dice, xy = found.popitem()
fprint('dice {}'.format(dice))
ray, base = history_tf[dice]
where = base + 3 * ray
fprint(where)
fprint('Moving!', msg_color='yellow')
fprint('Current position: {}'.format(sub.pose.position))
fprint('zrp')
yield sub.move.zero_roll_and_pitch().go(blind=True)
yield sub.nh.sleep(4)
fprint('hitting', msg_color='yellow')
yield sub.move.look_at(where).go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
yield sub.move.set_position(where).go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
fprint('going back', msg_color='yellow')
yield start.go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
def run(self, args):
global markers
markers = MarkerArray()
pub_markers = yield self.nh.advertise('/pinger/rays', MarkerArray)
fprint('Getting Guess Locations')
use_prediction = True
try:
pinger_txros = yield self.nh.get_service_client(
'/guess_location', GuessRequest)
pinger_1_req = yield pinger_txros(
GuessRequestRequest(item='pinger_surface'))
pinger_2_req = yield pinger_txros(
GuessRequestRequest(item='pinger_shooter'))
if pinger_1_req.found is False or pinger_2_req.found is False:
use_prediction = False
fprint('Forgot to add pinger to guess server?',
msg_color='yellow')
else:
fprint('Found two pinger guesses', msg_color='green')
pinger_guess = yield self.transform_to_baselink(
self, pinger_1_req, pinger_2_req)
fprint(pinger_guess)
except Exception as e:
fprint('Failed to /guess_location. Procceding without guess',
msg_color='yellow')
use_prediction = False
fprint(e)
while True:
fprint('=' * 50)
fprint("waiting for message")
p_message = yield self.pinger_sub.get_next_message()
pub_markers.publish(markers)
fprint(p_message)
# Ignore freuqnecy
if not abs(p_message.freq - FREQUENCY) < FREQUENCY_TOL:
fprint("Ignored! Recieved Frequency {}".format(p_message.freq),
msg_color='red')
if use_prediction:
fprint("Moving to random guess")
yield self.go_to_random_guess(self, pinger_1_req,
pinger_2_req)
continue
# Ignore magnitude from processed ping
p_position = mil_ros_tools.rosmsg_to_numpy(p_message.position)
vec = p_position / np.linalg.norm(p_position)
if np.isnan(vec).any():
fprint('Ignored! nan', msg_color='red')
if use_prediction:
yield self.go_to_random_guess(self, pinger_1_req,
pinger_2_req)
continue
# Tranform hydrophones vector to relative coordinate
transform = yield self._tf_listener.get_transform(
'/base_link', '/hydrophones')
vec = transform._q_mat.dot(vec)
fprint('Transformed vec: {}'.format(vec))
marker = Marker(ns='pinger',
action=visualization_msgs.Marker.ADD,
type=Marker.ARROW,
scale=Vector3(0.2, 0.5, 0),
points=np.array([
Point(0, 0, 0),
Point(vec[0], vec[1], vec[2])
]))
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)
# Check if we are on top of pinger
if abs(vec[0]) < POSITION_TOL and abs(
vec[1]) < POSITION_TOL and vec[2] < Z_POSITION_TOL:
if not use_prediction:
fprint("Arrived to pinger!")
break
sub_position, _ = yield self.tx_pose()
dists = [
np.linalg.norm(sub_position -
mil_ros_tools.rosmsg_to_numpy(
x.location.pose.position))
for x in (pinger_1_req, pinger_2_req)
]
pinger_id = np.argmin(dists)
# pinger_id 0 = pinger_surface
# pinger_id 1 = pinger_shooter
yield self.nh.set_param("pinger_where", int(pinger_id))
break
vec[2] = 0
if use_prediction:
pinger_guess = yield self.transform_to_baselink(
self, pinger_1_req, pinger_2_req)
fprint('Transformed guess: {}'.format(pinger_guess))
# Check if the pinger aligns with guess
# check, vec = self.check_with_guess(vec, pinger_guess)
fprint('move to {}'.format(vec))
yield self.fancy_move(self, vec)
fprint('Arrived to hydrophones! Going down!')
yield self.move.to_height(PINGER_HEIGHT).zero_roll_and_pitch().go(
speed=0.1)
def run(sub):
yield sub.move.downward(1).go()
fprint('Begin search for gates')
# Search 4 quadrants deperated by 90 degrees for the gate
start = sub.move.zero_roll_and_pitch()
# Pitch up and down to populate pointcloud
so = SonarObjects(sub, [start.pitch_down_deg(7), start] * 10)
transform = yield sub._tf_listener.get_transform('/map', '/base_link')
# [1, 0, 0] is front vector for sub
ray = transform._q_mat.dot(np.array([1, 0, 0]))
# Start scan and search
res = yield so.start_search_in_cone(transform._p,
ray,
angle_tol=60,
distance_tol=11,
speed=0.1,
clear=True,
c_func=find_gate)
del so
fprint('Found {} objects'.format(len(res.objects)))
# Didn't find enough objects
if len(res.objects) < 2:
fprint('No objects')
# Search for two objects that satisfy the gate
gate_points = find_gate(res.objects, ray)
if gate_points is None:
fprint('No valid gates')
if gate_points is None:
fprint('Returning')
yield start.go()
defer.returnValue(False)
print()
# Find midpoint between the two poles/objects
mid_point = gate_points[0] + gate_points[1]
mid_point = mid_point / 2
# Offset z so we don't hit the bar
mid_point[2] = mid_point[2] - 0.5
fprint('Midpoint: {}'.format(mid_point))
yield sub.move.look_at(mid_point).go(speed=SPEED)
fprint('Moving!', msg_color='yellow')
yield sub.move.set_position(mid_point).go(speed=SPEED)
normal = mid_point - sub.pose.position
normal[2] = 0
normal = normal / np.linalg.norm(normal)
fprint('Normal {} '.format(normal))
fprint('Moving past the gate', msg_color='yellow')
yield sub.move.set_position(mid_point +
DIST_AFTER_GATE * normal).go(speed=SPEED)
fprint('Checking if already seen qualification pole')
current_position = sub.pose.position
objects = SonarObjects._get_objects_within_cone(res.objects,
current_position,
normal,
angle_tol=100,
distance_tol=11)
objects = None
print(objects)
if objects is None or len(objects) < 1:
fprint('Searching for qualifiction pole')
start = sub.move.zero_roll_and_pitch()
so = SonarObjects(sub, [start.pitch_down_deg(7), start] * 4)
so_objects = yield so.start_search_in_cone(sub.pose.position,
normal,
angle_tol=50,
distance_tol=11,
speed=0.1,
clear=True)
objects = so_objects.objects
if objects is None:
fprint('Failed to find qualification pole')
defer.retuenValue(False)
fprint('Found pole')
objects = SonarObjects._sort_by_angle(objects, normal, current_position)
pole = rosmsg_to_numpy(objects[0].pose.position)
fprint(pole)
pole[2] = sub.pose.position[2]
normal_pole = pole - sub.pose.position
normal_pole = normal_pole / np.linalg.norm(normal_pole)
move1 = pole
move1 = move1 - normal_pole * 2
fprint(move1)
yield sub.move.set_position(move1).go(speed=SPEED_CAREFUL)
yield sub.move.left(1.7).go(speed=SPEED_CAREFUL)
yield sub.move.forward(1.7).go(speed=SPEED_CAREFUL)
yield sub.move.right(3.4).go(speed=SPEED_CAREFUL)
yield sub.move.backward(1.7).go(speed=SPEED_CAREFUL)
yield sub.move.left(1.7).go(speed=SPEED_CAREFUL)
yield sub.move.backward(0.5).go(speed=SPEED_CAREFUL)
yield sub.move.look_at(mid_point).go(speed=SPEED_CAREFUL)
fprint('Going to front of gate')
yield sub.move.set_position(mid_point +
DIST_AFTER_GATE * normal).go(speed=SPEED)
yield sub.move.set_position(mid_point).go(speed=SPEED_CAREFUL)
yield sub.move.set_position(mid_point -
DIST_AFTER_GATE * normal).go(speed=SPEED)
def run(sub):
global markers
markers = MarkerArray()
pub_markers = yield sub.nh.advertise('/pinger/rays', MarkerArray)
fprint('Getting Guess Locations')
pinger_txros = yield sub.nh.get_service_client('/guess_location',
GuessRequest)
pinger_1_req = yield pinger_txros(GuessRequestRequest(item='pinger1'))
pinger_2_req = yield pinger_txros(GuessRequestRequest(item='pinger2'))
use_prediction = True
if pinger_1_req.found is False or pinger_2_req.found is False:
use_prediction = False
fprint('Forgot to add pinger to guess server?', msg_color='yellow')
else:
fprint('Found two pinger guesses', msg_color='green')
pinger_guess = yield transform_to_baselink(sub, pinger_1_req,
pinger_2_req)
fprint(pinger_guess)
hydro_processed = yield sub.nh.subscribe('/hydrophones/processed',
ProcessedPing)
while True:
fprint('=' * 50)
pub_markers.publish(markers)
p_message = yield hydro_processed.get_next_message()
# Ignore freuqnecy
if not abs(p_message.freq - FREQUENCY) < FREQUENCY_TOL:
fprint(
"Ignored! Recieved Frequency {}".format(p_message.freq),
msg_color='red')
if use_prediction:
yield go_to_random_guess(sub, pinger_1_req, pinger_2_req)
continue
# Ignore magnitude from processed ping
p_position = mil_ros_tools.rosmsg_to_numpy(p_message.position)
vec = p_position / np.linalg.norm(p_position)
if np.isnan(vec).any():
fprint('Ignored! nan', msg_color='red')
if use_prediction:
yield go_to_random_guess(sub, pinger_1_req, pinger_2_req)
continue
# Tranform hydrophones vector to relative coordinate
transform = yield sub._tf_listener.get_transform(
'/base_link', '/hydrophones')
vec = transform._q_mat.dot(vec)
fprint('Transformed vec: {}'.format(vec))
marker = Marker(
ns='pinger',
action=visualization_msgs.Marker.ADD,
type=Marker.ARROW,
scale=Vector3(0.2, 0.5, 0),
points=np.array([Point(0, 0, 0),
Point(vec[0], vec[1], vec[2])]))
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)
# Check if we are on top of pinger
if abs(vec[0]) < POSITION_TOL and abs(vec[1] < POSITION_TOL):
sub_position, _ = yield sub.tx_pose()
dists = [
np.linalg.norm(sub_position - mil_ros_tools.rosmsg_to_numpy(
x.location.pose.position))
for x in (pinger_1_req, pinger_2_req)
]
pinger_id = np.argmin(dists)
yield sub.nh.set_param("pinger_where", int(pinger_id))
break
vec[2] = 0
if use_prediction:
pinger_guess = yield transform_to_baselink(sub, pinger_1_req,
pinger_2_req)
fprint('Transformed guess: {}'.format(pinger_guess))
# Check if the pinger aligns with guess
check, vec = check_with_guess(vec, pinger_guess)
fprint('move to {}'.format(vec))
yield fancy_move(sub, vec)
fprint('Arrived to hydrophones! Going down!')
yield sub.move.to_height(PINGER_HEIGHT).zero_roll_and_pitch().go(speed=0.1)
def from_Pose(cls, frame_id, msg):
return cls(frame_id, rosmsg_to_numpy(msg.position),
rosmsg_to_numpy(msg.orientation))
class BallDrop(SubjuGator):
@util.cancellableInlineCallbacks
def run(self, args):
try:
ree = rospy.ServiceProxy('/vision/garlic/enable', SetBool)
resp = ree(True)
if not resp:
print("Error, failed to init neural net.")
return
except rospy.ServiceException, e:
print("Service Call Failed")
fprint('Enabling cam_ray publisher')
yield self.nh.sleep(1)
fprint('Connecting camera')
cam_info_sub = yield self.nh.subscribe('/camera/down/camera_info',
CameraInfo)
fprint('Obtaining cam info message')
cam_info = yield cam_info_sub.get_next_message()
cam_center = np.array([cam_info.width / 2, cam_info.height / 2])
cam_norm = np.sqrt(cam_center[0]**2 + cam_center[1]**2)
fprint('Cam center: {}'.format(cam_center))
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)
ball_drop_req = yield vamp_txros(
GuessRequestRequest(item='ball_drop'))
if ball_drop_req.found is False:
use_prediction = False
fprint('Forgot to add ball_drop to guess?', msg_color='yellow')
else:
fprint('Found ball_drop.', msg_color='green')
yield self.move.set_position(
mil_ros_tools.rosmsg_to_numpy(
ball_drop_req.location.pose.position)).depth(
TRAVEL_DEPTH).go(speed=FAST_SPEED)
except Exception as e:
fprint(str(e) + 'Forgot to run guess server?', msg_color='yellow')
ball_drop_sub = yield self.nh.subscribe('/bbox_pub', Point)
yield self.move.to_height(SEARCH_HEIGHT).zero_roll_and_pitch().go(
speed=SPEED)
while True:
fprint('Getting location of ball drop...')
ball_drop_msg = yield ball_drop_sub.get_next_message()
ball_drop_xy = mil_ros_tools.rosmsg_to_numpy(ball_drop_msg)[:2]
vec = ball_drop_xy - cam_center
fprint("Vec: {}".format(vec))
vec = vec / cam_norm
vec[1] = -vec[1]
fprint("Rel move vec {}".format(vec))
if np.allclose(vec, np.asarray(0), atol=50):
break
vec = np.append(vec, 0)
yield self.move.relative_depth(vec).go(speed=SPEED)
fprint('Centered, going to depth {}'.format(HEIGHT_BALL_DROPER))
yield self.move.to_height(HEIGHT_BALL_DROPER).zero_roll_and_pitch().go(
speed=SPEED)
fprint('Dropping marker')
yield self.actuators.drop_marker()
def test_rosmsg_to_numpy_custom(self):
'''Test a rosmsg conversion for a custom msg'''
pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14)
numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta'])
np.testing.assert_allclose(np.array([1.0, 2.0, 3.14]), numpy_array)
