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 move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type), msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(rosmsg_to_numpy(msg.goal.orientation))[2]
if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type),
msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(rosmsg_to_numpy(
msg.goal.orientation))[2]
if not self.is_feasible(
np.array([msg.goal.position.x, msg.goal.position.y, yaw]),
np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def run(self, parameters):
result = self.fetch_result()
found_buoys = yield self.database_query("start_gate")
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
if len(buoys) == 2:
ogrid, setup, target = yield self.two_buoys(buoys, 10)
elif len(buoys) == 4:
ogrid, setup, target = yield self.four_buoys(buoys, 10)
else:
raise Exception
# Put the target into the point cloud as well
points = []
points.append(mil_tools.numpy_to_point(target.position))
pc = PointCloud(header=mil_tools.make_header(frame='/enu'),
points=np.array(points))
yield self._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield self.nh.sleep(3)
yield setup.go(move_type='skid')
pose = yield self.tx_pose
ogrid.generate_grid(pose)
msg = ogrid.get_message()
print "publishing"
self.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
print "START_GATE: Moving in 5 seconds!"
yield target.go(initial_plan_time=5)
defer.returnValue(result)
def get_message(self):
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten(), -100, 100).astype(np.int8)
return ogrid
def get_message(self):
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten(), -100, 100).astype(np.int8)
return ogrid
def position_to_object(self, position, color, id, name="totem"):
obj = PerceptionObject()
obj.id = int(id)
obj.header = mil_tools.make_header(frame="enu")
obj.name = name
obj.position = mil_tools.numpy_to_point(position)
obj.color.r = color[0]
obj.color.g = color[1]
obj.color.b = color[2]
obj.color.a = 1
return obj
def position_to_object(self, position, color, id, name="totem"):
obj = PerceptionObject()
obj.id = int(id)
obj.header = mil_tools.make_header(frame="enu")
obj.name = name
obj.position = mil_tools.numpy_to_point(position)
obj.color.r = color[0]
obj.color.g = color[1]
obj.color.b = color[2]
obj.color.a = 1
return obj
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 get_message(self):
if self.grid is None:
fprint("Ogrid was requested but no ogrid was found. Using blank.", msg_color='yellow')
self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def get_message(self):
if self.grid is None:
fprint("Ogrid was requested but no ogrid was found. Using blank.", msg_color='yellow')
self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = mil_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1])
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame='enu')
ogrid.info.origin = origin
ogrid.info.width = size / self.resolution
ogrid.info.height = size / self.resolution
ogrid.info.resolution = self.resolution
ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def __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 __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 _make_bounds(cls):
fprint("Constructing bounds.", title="NAVIGATOR")
if (yield cls.nh.has_param("/bounds/enforce")):
_bounds = cls.nh.get_service_client('/get_bounds',
navigator_srvs.Bounds)
yield _bounds.wait_for_service()
resp = yield _bounds(navigator_srvs.BoundsRequest())
if resp.enforce:
cls.enu_bounds = [
mil_tools.rosmsg_to_numpy(bound) for bound in resp.bounds
]
# Just for display
pc = PointCloud(header=mil_tools.make_header(frame='/enu'),
points=np.array([
mil_tools.numpy_to_point(point)
for point in cls.enu_bounds
]))
yield cls._point_cloud_pub.publish(pc)
else:
fprint("No bounds param found, defaulting to none.",
title="NAVIGATOR")
cls.enu_bounds = None
def as_MoveToGoal(self, linear=[0, 0, 0], angular=[0, 0, 0], **kwargs):
return MoveToGoal(header=make_header(),
posetwist=self.as_PoseTwist(linear, angular),
**kwargs)
def as_MoveToGoal(self, linear=[0, 0, 0], angular=[0, 0, 0], **kwargs):
return MoveToGoal(
header=make_header(),
posetwist=self.as_PoseTwist(linear, angular),
**kwargs
)
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)