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