def dynamic_cb(self, config, level):
fprint("OGRID DYNAMIC CONFIG UPDATE!", msg_color='blue')
topics = config['topics'].replace(' ', '').split(',')
new_grids = {}
for topic in topics:
new_grids[topic] = OGrid(topic) if topic not in self.ogrids else self.ogrids[topic]
self.ogrids = new_grids
map_size = map(int, (config['height'], config['width']))
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']
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 = navigator_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 = navigator_tools.numpy_quat_pair_to_pose(position, quaternion)
self.global_ogrid = self.create_grid(map_size)
return config
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 = navigator_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=navigator_tools.make_header(frame='/enu'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(*self.last_enu)
),
twist=TwistWithCovariance(
twist=twist
)
)
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(self.last_ecef, self.last_enu[1])
),
twist=TwistWithCovariance(
twist=twist
)
)
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 = navigator_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=navigator_tools.make_header(frame='/enu'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
*self.last_enu)),
twist=TwistWithCovariance(twist=twist))
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
self.last_ecef, self.last_enu[1])),
twist=TwistWithCovariance(twist=twist))
def state_cb(self, msg):
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
enu_pose = navigator_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=navigator_tools.make_header(frame='/enu'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
*self.last_enu)),
twist=TwistWithCovariance(twist=twist))
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
self.last_ecef, self.last_enu[1])),
twist=TwistWithCovariance(twist=twist))
def state_cb(self, msg):
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
enu_pose = navigator_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=navigator_tools.make_header(frame='/enu'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(*self.last_enu)
),
twist=TwistWithCovariance(
twist=twist
)
)
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_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.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 = navigator_tools.numpy_quat_pair_to_pose(
position, quaternion)
self.global_ogrid = self.create_grid((map_size, map_size))
set_odom = lambda msg: setattr(
self, 'odom', navigator_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber('/odom', Odometry, set_odom)
self.publisher = rospy.Publisher('/ogrid_master',
OccupancyGrid,
queue_size=1)
Server(OgridConfig, self.dynamic_cb)
dynam_client = Client("bounds_server", config_callback=self.bounds_cb)
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 = navigator_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, 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 = navigator_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.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 = navigator_tools.numpy_quat_pair_to_pose(position, quaternion)
self.global_ogrid = self.create_grid((map_size, map_size))
set_odom = lambda msg: setattr(self, 'odom', navigator_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber('/odom', Odometry, set_odom)
self.publisher = rospy.Publisher('/ogrid_master', OccupancyGrid, queue_size=1)
Server(OgridConfig, self.dynamic_cb)
dynam_client = Client("bounds_server", config_callback=self.bounds_cb)
rospy.Timer(rospy.Duration(1.0 / rate), self.publish)
def two_buoys(navigator, buoys, setup_dist, channel_length=30):
# We only have the front two buoys in view
pose = yield navigator.tx_pose
# Get the ones closest to us and assume those are the front
front = buoys
t = txros.tf.Transform.from_Pose_message(
navigator_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 = navigator.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 create_grid(self, map_size, resolution, origin=None):
'''
Creates blank ogrids for everyone for the low low price of $9.95!
`map_size` should be in the form of (h, w)
`resolution` should be in m/cell
'''
ogrid_ = OccupancyGrid()
ogrid_.header.stamp = rospy.Time.now()
ogrid_.header.frame_id = self.frame_id
ogrid_.info.resolution = resolution
ogrid_.info.width = map_size[1]
ogrid_.info.height = map_size[0]
if origin is None:
position = np.array([-(map_size[1] * resolution) / 2, -(map_size[0] * resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
origin = navigator_tools.numpy_quat_pair_to_pose(position, quaternion)
ogrid_.info.origin = origin
# TODO: Make sure this produces the correct sized ogrid
ogrid_.data = np.full((map_size[1], map_size[0]), -1).flatten()
fprint('Created Occupancy Map', msg_color='blue')
return ogrid_
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 = navigator_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 = navigator_tools.numpy_quat_pair_to_pose(
position, quaternion)
self.global_ogrid = self.create_grid(map_size)
return config
def make_ogrid_transform(self):
self.origin = navigator_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
# 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 make_ogrid_transform(self):
self.origin = navigator_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 make_ogrid_transform(self):
origin_x = self.center[0] - 30
origin_y = self.center[1] - 30
self.origin = navigator_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(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = navigator_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1])
grid = np.zeros((size / self.resolution, size / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = navigator_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):
origin_x = self.center[0] - 30
origin_y = self.center[1] - 30
self.origin = navigator_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 four_buoys(navigator, buoys, setup_dist):
pose = yield navigator.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(
navigator_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 = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
setup = navigator.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 __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 = navigator_tools.numpy_quat_pair_to_pose(start_pos, start_ori)
start_odom = Odometry()
start_odom.header = navigator_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 make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = navigator_tools.numpy_quat_pair_to_pose(
np_origin, [0, 0, 0, 1])
grid = np.zeros((size / self.resolution, size / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = navigator_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 __init__(self):
# Ogrid used to keep track of where we have explored
self.map_metadata = MapMetaData()
self.map_metadata.resolution = .5
self.map_metadata.width = 500
self.map_metadata.height = 500
origin = navigator_tools.numpy_quat_pair_to_pose([-125, -125, 0],
[0, 0, 0, 1])
self.map_metadata.origin = origin
self.reset_grid()
self.ogrid_pub = rospy.Publisher("/explorer/ogrid",
OccupancyGrid,
queue_size=1)
self.draw_shape = self._lidar_view
rospy.Timer(self.pub_ogrid, rospy.Duration(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 = navigator_tools.numpy_quat_pair_to_pose(
start_pos, start_ori)
start_odom = Odometry()
start_odom.header = navigator_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(navigator, buoys, setup_dist):
pose = yield navigator.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(navigator_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 = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
setup = navigator.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 two_buoys(navigator, buoys, setup_dist, channel_length=30):
# We only have the front two buoys in view
pose = yield navigator.tx_pose
# Get the ones closest to us and assume those are the front
front = buoys
t = txros.tf.Transform.from_Pose_message(navigator_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 = navigator.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 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 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 main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
if found_buoys.found:
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
else:
print "START_GATE: Error 4 - No db buoy response..."
result.success = False
result.response = result.DbObjectNotFound
result.message = "Start gates not found in the database."
return_with(result)
if len(buoys) != 4:
print "START_GATE: Error 5 - Invaild number of buoys found: {} (expecting 4)".format(len(buoys))
result.success = False
result.message = "Invaild number of buoys found: {} (expecting 4)".format(len(buoys))
return_with(result)
# buoys = [Buoy.from_srv(left), Buoy.from_srv(right)]
#buoys = np.array(get_buoys())
pose = yield navigator.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(navigator_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = 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)]
points = [navigator_tools.numpy_to_point(b.position) for b in [f_left_buoy]]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
through_vector = b_mid_point - f_mid_point
through_vector = through_vector / np.linalg.norm(through_vector)
#print mid_point
setup_dist = 20 # Line up with the start gate this many meters infront of the gate.
setup = f_mid_point - f_direction_vector * setup_dist
target = b_mid_point + through_vector * setup_dist
# Put the target into the point cloud as well
#points.append(navigator_tools.numpy_to_point(b_left_buoy))
points.append(navigator_tools.numpy_to_point(target))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield navigator.move.set_position(setup).look_at(f_mid_point).go(move_type="skid")
pose = yield navigator.tx_pose
ogrid = OgridFactory(f_left_buoy.position, f_right_buoy.position, pose[0],
target, left_b_pos=b_left_buoy.position, right_b_pos=b_right_buoy.position)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
yield navigator.nh.sleep(5)
print "START_GATE: Moving in 5 seconds!"
yield navigator.move.set_position(target).go(initial_plan_time=5)
return_with(result)
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
'''
left_point = None
right_point = None
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
navigator_tools.draw_ray_3d(red_left_pt,
left_cam, [1, 0, 0, 1],
m_id=0,
frame="front_left_cam")
navigator_tools.draw_ray_3d(red_right_pt,
right_cam, [1, 0, 0, 1],
m_id=1,
frame="front_right_cam")
navigator_tools.draw_ray_3d(green_left_pt,
left_cam, [0, 1, 0, 1],
m_id=2,
frame="front_left_cam")
navigator_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 = navigator_tools.make_header(frame="enu")
red_point.point = navigator_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = navigator_tools.make_header(frame="enu")
green_point.point = navigator_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 = navigator_tools.make_header(frame="enu")
target_pose.pose = navigator_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
if found_buoys.found:
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
else:
print "START_GATE: Error 4 - No db buoy response..."
result.success = False
result.response = result.DbObjectNotFound
result.message = "Start gates not found in the database."
return_with(result)
if len(buoys) != 4:
print "START_GATE: Error 5 - Invaild number of buoys found: {} (expecting 4)".format(
len(buoys))
result.success = False
result.message = "Invaild number of buoys found: {} (expecting 4)".format(
len(buoys))
return_with(result)
# buoys = [Buoy.from_srv(left), Buoy.from_srv(right)]
#buoys = np.array(get_buoys())
pose = yield navigator.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(
navigator_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = 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)]
points = [
navigator_tools.numpy_to_point(b.position) for b in [f_left_buoy]
]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
through_vector = b_mid_point - f_mid_point
through_vector = through_vector / np.linalg.norm(through_vector)
#print mid_point
setup_dist = 20 # Line up with the start gate this many meters infront of the gate.
setup = f_mid_point - f_direction_vector * setup_dist
target = b_mid_point + through_vector * setup_dist
# Put the target into the point cloud as well
#points.append(navigator_tools.numpy_to_point(b_left_buoy))
points.append(navigator_tools.numpy_to_point(target))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield navigator.move.set_position(setup).look_at(f_mid_point).go(
move_type="skid")
pose = yield navigator.tx_pose
ogrid = OgridFactory(f_left_buoy.position,
f_right_buoy.position,
pose[0],
target,
left_b_pos=b_left_buoy.position,
right_b_pos=b_right_buoy.position)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
yield navigator.nh.sleep(5)
print "START_GATE: Moving in 5 seconds!"
yield navigator.move.set_position(target).go(initial_plan_time=5)
return_with(result)
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
'''
left_point = None
right_point = None
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", "stereo_left_cam", rospy.Time(), rospy.Duration(4.0))
left_image, right_image = left.frame, right.frame
cam_tf = tf_listener.lookupTransform("stereo_left_cam", "stereo_right_cam", left.sub.last_image_time)
cam_p, cam_q = tf_listener.lookupTransform("enu", "stereo_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
navigator_tools.draw_ray_3d(red_left_pt, left_cam, [1, 0, 0, 1], m_id=0, frame="stereo_left_cam")
navigator_tools.draw_ray_3d(red_right_pt, right_cam, [1, 0, 0, 1], m_id=1, frame="stereo_right_cam")
navigator_tools.draw_ray_3d(green_left_pt, left_cam, [0, 1, 0, 1], m_id=2, frame="stereo_left_cam")
navigator_tools.draw_ray_3d(green_right_pt, right_cam, [0, 1, 0, 1], m_id=3, frame="stereo_right_cam")
red_point = PointStamped()
red_point.header = navigator_tools.make_header(frame="enu")
red_point.point = navigator_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = navigator_tools.make_header(frame="enu")
green_point.point = navigator_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 = navigator_tools.make_header(frame="enu")
target_pose.pose = navigator_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def _publish_pose(pub, pose):
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "enu"
pose = nt.numpy_quat_pair_to_pose(pose.pose[0], pose.pose[1])
pose_stamped.pose = pose
pub.publish(pose_stamped)
