def state_cb(self, msg):
if rospy.Time.now() - self.last_state_sub_time < self.state_sub_max_prd:
return
self.last_state_sub_time = rospy.Time.now()
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
enu_pose = mil_tools.pose_to_numpy(msg.pose[target_index])
self.last_ecef = self.enu_to_ecef(enu_pose[0])
self.last_enu = enu_pose
self.last_odom = Odometry(
header=mil_tools.make_header(frame='/enu'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=mil_tools.numpy_quat_pair_to_pose(*self.last_enu)
),
twist=TwistWithCovariance(
twist=twist
)
)
self.last_absodom = Odometry(
header=mil_tools.make_header(frame='/ecef'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=mil_tools.numpy_quat_pair_to_pose(self.last_ecef, self.last_enu[1])
),
twist=TwistWithCovariance(
twist=twist
)
)
def __init__(self, frame_id='enu', map_size=500, resolution=0.3, rate=1):
self.frame_id = frame_id
self.ogrids = {}
self.odom = None
# Some default values
self.plow = True
self.plow_factor = 0
self.ogrid_min_value = -1
self.draw_bounds = False
self.resolution = resolution
self.ogrid_timeout = 2
self.enforce_bounds = False
self.enu_bounds = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]]
# Default to centering the ogrid
position = np.array([-(map_size * resolution) / 2, -(map_size * resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion)
self.global_ogrid = self.create_grid((map_size, map_size))
def set_odom(msg):
return setattr(self, 'odom', mil_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber('/odom', Odometry, set_odom)
self.publisher = rospy.Publisher('/ogrid_master', OccupancyGrid, queue_size=1)
self.ogrid_server = Server(OgridConfig, self.dynamic_cb)
self.dynam_client = Client("bounds_server", config_callback=self.bounds_cb)
rospy.Service("~center_ogrid", Trigger, self.center_ogrid)
rospy.Timer(rospy.Duration(1.0 / rate), self.publish)
def main(navigator):
waypoints = []
poses = []
joy = yield navigator.nh.subscribe("/joy", Joy)
waypoint_pub = yield navigator.nh.advertise("/mission_waypoints", PoseArray)
last_set = False
print "Waiting for input. RB to set waypoint, right D-Pad to go."
navigator.change_wrench("rc")
while True:
joy_msg = yield joy.get_next_message()
b_set = bool(joy_msg.buttons[5]) # RB
b_go = bool(joy_msg.buttons[12]) # Right D-Pad
if b_set and b_set != last_set:
# Set a waypoint at the present location
waypoints.append(navigator.move)
# For displaying a pose array
poses.append(numpy_quat_pair_to_pose(*navigator.pose))
pa = PoseArray(header=Header(frame_id="enu"), poses=poses)
print "SET"
yield waypoint_pub.publish(pa)
last_set = b_set
if b_go and len(waypoints) > 1:
print "GOING"
break
for waypoint in itertools.cycle(waypoints):
yield waypoint.go()
print "Arrived!"
yield navigator.nh.sleep(5)
def two_buoys(self, buoys, setup_dist, channel_length=30):
# We only have the front two buoys in view
pose = yield self.tx_pose
# Get the ones closest to us and assume those are the front
front = buoys
t = txros.tf.Transform.from_Pose_message(mil_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = np.linalg.inv(t.as_matrix())
f_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in front]
# print f_bl_buoys
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array([np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
# print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[np.argmin(angle_buoys)]
between_v = f_right_buoy.position - f_left_buoy.position
f_mid_point = f_left_buoy.position + between_v / 2
setup = self.move.set_position(f_mid_point).look_at(f_left_buoy.position).yaw_right(1.57).backward(setup_dist)
target = setup.forward(2 * setup_dist + channel_length)
ogrid = OgridFactory(f_left_buoy.position, f_right_buoy.position,
target.position, channel_length=channel_length)
defer.returnValue([ogrid, setup, target])
def __init__(self, bf_size=60, min_t_spacing=9, num_of_buoys=20):
self.ogrid_pub = rospy.Publisher('/ogrid', OccupancyGrid, queue_size=2)
self.odom = DoOdom(bf_size)
self.bf_size = bf_size
self.min_t_spacing = min_t_spacing
self.num_of_buoys = num_of_buoys
# Some ogrid defs
self.grid = None
self.resolution = 0.3
self.height = bf_size * 3
self.width = bf_size * 3
self.origin = mil_tools.numpy_quat_pair_to_pose([-bf_size, -bf_size, 0],
[0, 0, 0, 1])
self.publish_ogrid = lambda *args: self.ogrid_pub.publish(self.get_message())
self.buoy_size = 1 # radius of buoy (m)
self.totem_size = 1 # radius of totem (m)
self.reseed(None)
self.draw_buoys()
self.draw_totems()
self.publish_ogrid()
# Now set up the database request service
rospy.Service("/database/requests", ObjectDBQuery, self.got_request)
rospy.Service("/reseed", Trigger, self.reseed)
rospy.Timer(rospy.Duration(1), self.publish_ogrid)
def __init__(self, frame_id='enu', map_size=500, resolution=0.3, rate=1):
self.frame_id = frame_id
self.ogrids = {}
self.odom = None
# Some default values
self.plow = True
self.plow_factor = 0
self.ogrid_min_value = -1
self.draw_bounds = False
self.resolution = resolution
self.ogrid_timeout = 2
self.enforce_bounds = False
self.enu_bounds = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]]
# Default to centering the ogrid
position = np.array([-(map_size * resolution) / 2, -(map_size * resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion)
self.global_ogrid = self.create_grid((map_size, map_size))
def set_odom(msg):
return setattr(self, 'odom', mil_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber('/odom', Odometry, set_odom)
self.publisher = rospy.Publisher('/ogrid_master', OccupancyGrid, queue_size=1)
self.ogrid_server = Server(OgridConfig, self.dynamic_cb)
self.dynam_client = Client("bounds_server", config_callback=self.bounds_cb)
self.ogrid_server.update_configuration({'width': 500})
rospy.Service("/center_ogrid", Trigger, self.center_ogrid)
rospy.Timer(rospy.Duration(1.0 / rate), self.publish)
def __init__(self, bf_size=60, min_t_spacing=9, num_of_buoys=20):
self.ogrid_pub = rospy.Publisher('/ogrid', OccupancyGrid, queue_size=2)
self.odom = DoOdom(bf_size)
self.bf_size = bf_size
self.min_t_spacing = min_t_spacing
self.num_of_buoys = num_of_buoys
# Some ogrid defs
self.grid = None
self.resolution = 0.3
self.height = bf_size * 3
self.width = bf_size * 3
self.origin = mil_tools.numpy_quat_pair_to_pose([-bf_size, -bf_size, 0],
[0, 0, 0, 1])
self.publish_ogrid = lambda *args: self.ogrid_pub.publish(self.get_message())
self.buoy_size = 1 # radius of buoy (m)
self.totem_size = 1 # radius of totem (m)
self.reseed(None)
self.draw_buoys()
self.draw_totems()
self.publish_ogrid()
# Now set up the database request service
rospy.Service("/database/requests", ObjectDBQuery, self.got_request)
rospy.Service("/reseed", Trigger, self.reseed)
rospy.Timer(rospy.Duration(1), self.publish_ogrid)
def two_buoys(self, buoys, setup_dist, channel_length=30):
# We only have the front two buoys in view
pose = yield self.tx_pose
# Get the ones closest to us and assume those are the front
front = buoys
t = txros.tf.Transform.from_Pose_message(
mil_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = np.linalg.inv(t.as_matrix())
f_bl_buoys = [
t_mat44.dot(np.append(buoy.position, 1)) for buoy in front
]
# print f_bl_buoys
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array(
[np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
# print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[
np.argmin(angle_buoys)]
between_v = f_right_buoy.position - f_left_buoy.position
f_mid_point = f_left_buoy.position + between_v / 2
setup = self.move.set_position(f_mid_point).look_at(
f_left_buoy.position).yaw_right(1.57).backward(setup_dist)
target = setup.forward(2 * setup_dist + channel_length)
ogrid = OgridFactory(f_left_buoy.position,
f_right_buoy.position,
target.position,
channel_length=channel_length)
defer.returnValue([ogrid, setup, target])
def dynamic_cb(self, config, level):
fprint("OGRID DYNAMIC CONFIG UPDATE!", msg_color='blue')
topics = config['topics'].replace(' ', '').split(',')
replace_topics = config['replace_topics'].replace(' ', '').split(',')
new_grids = {}
for topic in topics:
new_grids[topic] = OGrid(
topic) if topic not in self.ogrids else self.ogrids[topic]
for topic in replace_topics:
new_grids[topic] = OGrid(
topic, replace=True
) if topic not in self.ogrids else self.ogrids[topic]
self.ogrids = new_grids
map_size = map(int, (config['height'], config['width']))
self.map_size = map_size
self.plow = config['plow']
self.plow_factor = config['plow_factor']
self.ogrid_min_value = config['ogrid_min_value']
self.draw_bounds = config['draw_bounds']
self.resolution = config['resolution']
self.ogrid_timeout = config['ogrid_timeout']
if config['set_origin']:
fprint("Setting origin!")
position = np.array([config['origin_x'], config['origin_y'], 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(
position, quaternion)
else:
position = np.array([
-(map_size[1] * self.resolution) / 2,
-(map_size[0] * self.resolution) / 2, 0
])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(
position, quaternion)
self.global_ogrid = self.create_grid(map_size)
return config
def four_buoys(self, buoys, setup_dist):
pose = yield self.tx_pose
# Get the ones closest to us and assume those are the front
distances = np.array([b.distance(pose[0]) for b in buoys])
back = buoys[np.argsort(distances)[-2:]]
front = buoys[np.argsort(distances)[:2]]
# print "front", front
# print "back", back
# Made it this far, make sure the red one is on the left and green on the right ================
t = txros.tf.Transform.from_Pose_message(
mil_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = np.linalg.inv(t.as_matrix())
f_bl_buoys = [
t_mat44.dot(np.append(buoy.position, 1)) for buoy in front
]
b_bl_buoys = [
t_mat44.dot(np.append(buoy.position, 1)) for buoy in back
]
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array(
[np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
# print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[
np.argmin(angle_buoys)]
angle_buoys = np.array(
[np.arctan2(buoy[1], buoy[0]) for buoy in b_bl_buoys])
b_left_buoy, b_right_buoy = back[np.argmax(angle_buoys)], back[
np.argmin(angle_buoys)]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = self.get_path(
f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = self.get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
setup = self.move.set_position(f_mid_point).look_at(
b_mid_point).backward(setup_dist)
target = setup.set_position(b_mid_point).forward(setup_dist)
ogrid = OgridFactory(f_left_buoy.position,
f_right_buoy.position,
target.position,
left_b_pos=b_left_buoy.position,
right_b_pos=b_right_buoy.position)
defer.returnValue([ogrid, setup, target])
def make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = mil_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1])
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame='enu')
ogrid.info.origin = origin
ogrid.info.width = size / self.resolution
ogrid.info.height = size / self.resolution
ogrid.info.resolution = self.resolution
ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = mil_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1])
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame='enu')
ogrid.info.origin = origin
ogrid.info.width = size / self.resolution
ogrid.info.height = size / self.resolution
ogrid.info.resolution = self.resolution
ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def make_ogrid_transform(self):
self.origin = mil_tools.numpy_quat_pair_to_pose([self.x_lower, self.y_lower, 0],
[0, 0, 0, 1])
dx = self.x_upper - self.x_lower
dy = self.y_upper - self.y_lower
_max = max(dx, dy)
dx = dy = _max
# The grid needs to have it's axes swaped since its row major
self.grid = np.zeros((dy / self.resolution, dx / self.resolution))
# Transforms points from ENU to ogrid frame coordinates
self.t = np.array([[1 / self.resolution, 0, -self.x_lower / self.resolution],
[0, 1 / self.resolution, -self.y_lower / self.resolution],
[0, 0, 1]])
self.transform = lambda point: self.t.dot(np.append(point[:2], 1))[:2]
def dynamic_cb(self, config, level):
fprint("OGRID DYNAMIC CONFIG UPDATE!", msg_color='blue')
topics = config['topics'].replace(' ', '').split(',')
replace_topics = config['replace_topics'].replace(' ', '').split(',')
new_grids = {}
for topic in topics:
new_grids[topic] = OGrid(topic) if topic not in self.ogrids else self.ogrids[topic]
for topic in replace_topics:
new_grids[topic] = OGrid(topic, replace=True) if topic not in self.ogrids else self.ogrids[topic]
self.ogrids = new_grids
map_size = map(int, (config['height'], config['width']))
self.map_size = map_size
self.plow = config['plow']
self.plow_factor = config['plow_factor']
self.ogrid_min_value = config['ogrid_min_value']
self.draw_bounds = config['draw_bounds']
self.resolution = config['resolution']
self.ogrid_timeout = config['ogrid_timeout']
if config['set_origin']:
fprint("Setting origin!")
position = np.array([config['origin_x'], config['origin_y'], 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion)
else:
position = np.array([-(map_size[1] * self.resolution) / 2, -(map_size[0] * self.resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion)
self.global_ogrid = self.create_grid(map_size)
return config
def make_ogrid_transform(self):
origin_x = self.center[0] - 30
origin_y = self.center[1] - 30
self.origin = mil_tools.numpy_quat_pair_to_pose([origin_x, origin_y, 0],
[0, 0, 0, 1])
# The grid needs to have it's axes swaped since its row major
self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution)) - 1
# Transforms points from ENU to ogrid frame coordinates
self.t = np.array([[1 / self.resolution, 0, -origin_x / self.resolution],
[0, 1 / self.resolution, -origin_y / self.resolution],
[0, 0, 1]])
self.transform = lambda point: self.t.dot(np.append(point[:2], 1))[:2]
def make_ogrid_transform(self):
self.origin = mil_tools.numpy_quat_pair_to_pose(
[self.x_lower, self.y_lower, 0], [0, 0, 0, 1])
dx = self.x_upper - self.x_lower
dy = self.y_upper - self.y_lower
_max = max(dx, dy)
dx = dy = _max
# The grid needs to have it's axes swaped since its row major
self.grid = np.zeros((dy / self.resolution, dx / self.resolution))
# Transforms points from ENU to ogrid frame coordinates
self.t = np.array(
[[1 / self.resolution, 0, -self.x_lower / self.resolution],
[0, 1 / self.resolution, -self.y_lower / self.resolution],
[0, 0, 1]])
self.transform = lambda point: self.t.dot(np.append(point[:2], 1))[:2]
def __init__(self, rand_size):
self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
self.odom = None
self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry, self.set_odom)
fprint("Shaking hands and taking names.")
rospy.sleep(1)
# We need to publish an inital odom message for lqrrt
start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1)
start_pose = mil_tools.numpy_quat_pair_to_pose(start_pos, start_ori)
start_odom = Odometry()
start_odom.header = mil_tools.make_header(frame='enu')
start_odom.child_frame_id = 'base_link'
start_odom.pose.pose = start_pose
self.odom_pub.publish(start_odom)
def __init__(self, rand_size):
self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
self.odom = None
self.carrot_sub = rospy.Subscriber("/trajectory/cmd", Odometry, self.set_odom)
fprint("Shaking hands and taking names.")
rospy.sleep(1)
# We need to publish an inital odom message for lqrrt
start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1)
start_pose = mil_tools.numpy_quat_pair_to_pose(start_pos, start_ori)
start_odom = Odometry()
start_odom.header = mil_tools.make_header(frame='enu')
start_odom.child_frame_id = 'base_link'
start_odom.pose.pose = start_pose
self.odom_pub.publish(start_odom)
def four_buoys(self, buoys, setup_dist):
pose = yield self.tx_pose
# Get the ones closest to us and assume those are the front
distances = np.array([b.distance(pose[0]) for b in buoys])
back = buoys[np.argsort(distances)[-2:]]
front = buoys[np.argsort(distances)[:2]]
# print "front", front
# print "back", back
# Made it this far, make sure the red one is on the left and green on the right ================
t = txros.tf.Transform.from_Pose_message(mil_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = np.linalg.inv(t.as_matrix())
f_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in front]
b_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in back]
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array([np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
# print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[np.argmin(angle_buoys)]
angle_buoys = np.array([np.arctan2(buoy[1], buoy[0]) for buoy in b_bl_buoys])
b_left_buoy, b_right_buoy = back[np.argmax(angle_buoys)], back[np.argmin(angle_buoys)]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = self.get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = self.get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
setup = self.move.set_position(f_mid_point).look_at(b_mid_point).backward(setup_dist)
target = setup.set_position(b_mid_point).forward(setup_dist)
ogrid = OgridFactory(f_left_buoy.position, f_right_buoy.position, target.position,
left_b_pos=b_left_buoy.position, right_b_pos=b_right_buoy.position)
defer.returnValue([ogrid, setup, target])
def main(navigator):
waypoints = []
poses = []
joy = yield navigator.nh.subscribe("/joy", Joy)
waypoint_pub = yield navigator.nh.advertise("/mission_waypoints",
PoseArray)
last_set = False
print "Waiting for input. RB to set waypoint, right D-Pad to go."
navigator.change_wrench("rc")
while True:
joy_msg = yield joy.get_next_message()
b_set = bool(joy_msg.buttons[5]) # RB
b_go = bool(joy_msg.buttons[12]) # Right D-Pad
if b_set and b_set != last_set:
# Set a waypoint at the present location
waypoints.append(navigator.move)
# For displaying a pose array
poses.append(numpy_quat_pair_to_pose(*navigator.pose))
pa = PoseArray(header=Header(frame_id="enu"), poses=poses)
print "SET"
yield waypoint_pub.publish(pa)
last_set = b_set
if b_go and len(waypoints) > 1:
print "GOING"
break
for waypoint in itertools.cycle(waypoints):
yield waypoint.go()
print "Arrived!"
yield navigator.nh.sleep(5)
def do_buoys(srv, left, right, red_seg, green_seg, tf_listener):
'''
FYI:
`left`: the left camera ImageGetter object
`right`: the right camera ImageGetter object
'''
while not rospy.is_shutdown():
# Get all the required TF links
try:
# Working copy of the current frame obtained at the same time as the tf link
tf_listener.waitForTransform("enu", "front_left_cam", rospy.Time(),
rospy.Duration(4.0))
left_image, right_image = left.frame, right.frame
cam_tf = tf_listener.lookupTransform("front_left_cam",
"front_right_cam",
left.sub.last_image_time)
cam_p, cam_q = tf_listener.lookupTransform(
"enu", "front_left_cam", left.sub.last_image_time)
cam_p = np.array([cam_p])
cam_r = tf.transformations.quaternion_matrix(cam_q)[:3, :3]
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, TypeError) as e:
print e
rospy.logwarn("TF link not found.")
time.sleep(.5)
continue
red_left_pt, rl_area = red_seg.segment(left_image)
red_right_pt, rr_area = red_seg.segment(right_image)
green_left_pt, gl_area = green_seg.segment(left_image)
green_right_pt, gr_area = green_seg.segment(right_image)
area_tol = 50
if np.linalg.norm(rl_area - rr_area) > area_tol or np.linalg.norm(
gl_area - gr_area) > area_tol:
rospy.logwarn("Unsafe segmentation")
StartGateResponse(success=False)
red_point_np = do_the_magic(red_left_pt, red_right_pt, cam_tf)
red_point_np = np.array(cam_r.dot(red_point_np) + cam_p)[0]
green_point_np = do_the_magic(green_left_pt, green_right_pt, cam_tf)
green_point_np = np.array(cam_r.dot(green_point_np) + cam_p)[0]
# Just for visualization
for i in range(5):
# Publish it 5 times so we can see it in rviz
mil_tools.draw_ray_3d(red_left_pt,
left_cam, [1, 0, 0, 1],
m_id=0,
frame="front_left_cam")
mil_tools.draw_ray_3d(red_right_pt,
right_cam, [1, 0, 0, 1],
m_id=1,
frame="front_right_cam")
mil_tools.draw_ray_3d(green_left_pt,
left_cam, [0, 1, 0, 1],
m_id=2,
frame="front_left_cam")
mil_tools.draw_ray_3d(green_right_pt,
right_cam, [0, 1, 0, 1],
m_id=3,
frame="front_right_cam")
red_point = PointStamped()
red_point.header = mil_tools.make_header(frame="enu")
red_point.point = mil_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = mil_tools.make_header(frame="enu")
green_point.point = mil_tools.numpy_to_point(green_point_np)
green_pub.publish(green_point)
time.sleep(1)
# Now we have two points, find their midpoint and calculate a target angle
midpoint = (red_point_np + green_point_np) / 2
between_vector = green_point_np - red_point_np # Red is on the left when we go out.
yaw_theta = np.arctan2(between_vector[1], between_vector[0])
# Rotate that theta by 90 deg to get the angle through the buoys
yaw_theta += np.pi / 2.0
p = midpoint
q = tf.transformations.quaternion_from_euler(0, 0, yaw_theta)
target_pose = PoseStamped()
target_pose.header = mil_tools.make_header(frame="enu")
target_pose.pose = mil_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def do_buoys(srv, left, right, red_seg, green_seg, tf_listener):
'''
FYI:
`left`: the left camera ImageGetter object
`right`: the right camera ImageGetter object
'''
while not rospy.is_shutdown():
# Get all the required TF links
try:
# Working copy of the current frame obtained at the same time as the tf link
tf_listener.waitForTransform("enu", "front_left_cam", rospy.Time(), rospy.Duration(4.0))
left_image, right_image = left.frame, right.frame
cam_tf = tf_listener.lookupTransform("front_left_cam", "front_right_cam", left.sub.last_image_time)
cam_p, cam_q = tf_listener.lookupTransform("enu", "front_left_cam", left.sub.last_image_time)
cam_p = np.array([cam_p])
cam_r = tf.transformations.quaternion_matrix(cam_q)[:3, :3]
break
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, TypeError) as e:
print e
rospy.logwarn("TF link not found.")
time.sleep(.5)
continue
red_left_pt, rl_area = red_seg.segment(left_image)
red_right_pt, rr_area = red_seg.segment(right_image)
green_left_pt, gl_area = green_seg.segment(left_image)
green_right_pt, gr_area = green_seg.segment(right_image)
area_tol = 50
if np.linalg.norm(rl_area - rr_area) > area_tol or np.linalg.norm(gl_area - gr_area) > area_tol:
rospy.logwarn("Unsafe segmentation")
StartGateResponse(success=False)
red_point_np = do_the_magic(red_left_pt, red_right_pt, cam_tf)
red_point_np = np.array(cam_r.dot(red_point_np) + cam_p)[0]
green_point_np = do_the_magic(green_left_pt, green_right_pt, cam_tf)
green_point_np = np.array(cam_r.dot(green_point_np) + cam_p)[0]
# Just for visualization
for i in range(5):
# Publish it 5 times so we can see it in rviz
mil_tools.draw_ray_3d(red_left_pt, left_cam, [1, 0, 0, 1], m_id=0, frame="front_left_cam")
mil_tools.draw_ray_3d(red_right_pt, right_cam, [1, 0, 0, 1], m_id=1, frame="front_right_cam")
mil_tools.draw_ray_3d(green_left_pt, left_cam, [0, 1, 0, 1], m_id=2, frame="front_left_cam")
mil_tools.draw_ray_3d(green_right_pt, right_cam, [0, 1, 0, 1], m_id=3, frame="front_right_cam")
red_point = PointStamped()
red_point.header = mil_tools.make_header(frame="enu")
red_point.point = mil_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = mil_tools.make_header(frame="enu")
green_point.point = mil_tools.numpy_to_point(green_point_np)
green_pub.publish(green_point)
time.sleep(1)
# Now we have two points, find their midpoint and calculate a target angle
midpoint = (red_point_np + green_point_np) / 2
between_vector = green_point_np - red_point_np # Red is on the left when we go out.
yaw_theta = np.arctan2(between_vector[1], between_vector[0])
# Rotate that theta by 90 deg to get the angle through the buoys
yaw_theta += np.pi / 2.0
p = midpoint
q = tf.transformations.quaternion_from_euler(0, 0, yaw_theta)
target_pose = PoseStamped()
target_pose.header = mil_tools.make_header(frame="enu")
target_pose.pose = mil_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
