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): 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)