def to_pose(self, pose):
''' Takes a Pose or PoseStamped '''
if isinstance(pose, PoseStamped):
pose = pose.pose
position, orientation = mil_tools.pose_to_numpy(pose)
return PoseEditor2(self.nav, [position, orientation])
def is_feasible(self, x, u):
"""
Given a state x and effort u, returns a bool
that is only True if that (x, u) is feasible.
"""
# If there's no ogrid yet or it's a blind move, anywhere is valid
if self.ogrid is None or self.blind:
return True
# Body to world
c, s = np.cos(x[2]), np.sin(x[2])
R = np.array([[c, -s], [s, c]])
# Vehicle points in world frame
points = x[:2] + R.dot(params.vps).T
# Check for collision
cpm = 1 / self.ogrid.info.resolution
origin = pose_to_numpy(self.ogrid.info.origin)[0][:2]
indicies = (cpm * (points - origin)).astype(np.int64)
try:
data = np.array(self.ogrid.data).reshape(self.ogrid.info.width,
self.ogrid.info.height)
grid_values = data[indicies[:, 1], indicies[:, 0]]
except IndexError:
return False
# Greater than threshold is a hit
return np.all(grid_values < 90)
def is_feasible(self, x, u):
"""
Given a state x and effort u, returns a bool
that is only True if that (x, u) is feasible.
"""
# If there's no ogrid yet or it's a blind move, anywhere is valid
if self.ogrid is None or self.blind:
return True
# Body to world
c, s = np.cos(x[2]), np.sin(x[2])
R = np.array([[c, -s],
[s, c]])
# Vehicle points in world frame
points = x[:2] + R.dot(params.vps).T
# Check for collision
cpm = 1 / self.ogrid.info.resolution
origin = pose_to_numpy(self.ogrid.info.origin)[0][:2]
indicies = (cpm * (points - origin)).astype(np.int64)
try:
data = np.array(self.ogrid.data).reshape(self.ogrid.info.width, self.ogrid.info.height)
grid_values = data[indicies[:, 1], indicies[:, 0]]
except IndexError:
return False
# Greater than threshold is a hit
return np.all(grid_values < 90)
def to_pose(self, pose):
''' Takes a Pose or PoseStamped '''
if isinstance(pose, PoseStamped):
pose = pose.pose
position, orientation = mil_tools.pose_to_numpy(pose)
return PoseEditor2(self.nav, [position, orientation])
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 make_ogrid_transform(ogrid):
"""
Returns a matrix that transforms a point in ENU to this ogrid.
Invert the result to get ogrid -> ENU.
"""
resolution = ogrid.info.resolution
origin = mil_tools.pose_to_numpy(ogrid.info.origin)[0]
# Transforms points from ENU to ogrid frame coordinates
t = np.array([[1 / resolution, 0, -origin[0] / resolution],
[0, 1 / resolution, -origin[1] / resolution], [0, 0, 1]])
return t
def make_ogrid_transform(ogrid):
"""
Returns a matrix that transforms a point in ENU to this ogrid.
Invert the result to get ogrid -> ENU.
"""
resolution = ogrid.info.resolution
origin = mil_tools.pose_to_numpy(ogrid.info.origin)[0]
# Transforms points from ENU to ogrid frame coordinates
t = np.array([[1 / resolution, 0, -origin[0] / resolution],
[0, 1 / resolution, -origin[1] / resolution],
[0, 0, 1]])
return t
def run(self, args):
self.bridge = CvBridge()
self.image_debug_pub = self.nh.advertise('/dock_mask_debug', Image)
self.init_front_left_camera()
self.init_front_right_camera()
args = str.split(args, ' ')
self.color = args[0]
self.shape = args[1]
print("entered docking task", self.color, self.shape)
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=False))
info = yield self.front_left_camera_info_sub.get_next_message()
self.camera_model.fromCameraInfo(info)
pcodar_cluster_tol = DoubleParameter()
pcodar_cluster_tol.name = 'cluster_tolerance_m'
pcodar_cluster_tol.value = 10
yield self.pcodar_set_params(doubles = [pcodar_cluster_tol])
self.nh.sleep(5)
#find dock approach it
pos = yield self.find_dock()
print("going towards dock")
yield self.move.look_at(pos).set_position(pos).backward(20).go()
#Decrease cluster tolerance as we approach dock since lidar points are more dense
#This helps scenario where stc buoy is really close to dock
pcodar_cluster_tol = DoubleParameter()
pcodar_cluster_tol.name = 'cluster_tolerance_m'
pcodar_cluster_tol.value = 4
yield self.pcodar_set_params(doubles = [pcodar_cluster_tol])
self.nh.sleep(5)
# get a vector to the longer side of the dock
dock, pos = yield self.get_sorted_objects(name='dock', n=1)
dock = dock[0]
position, quat = pose_to_numpy(dock.pose)
rotation = quaternion_matrix(quat)
bbox = rosmsg_to_numpy(dock.scale)
bbox[2] = 0
max_dim = np.argmax(bbox[:2])
bbox[max_dim] = 0
bbox_enu = np.dot(rotation[:3,:3],bbox)
#this black magic uses the property that a rotation matrix is just a
#rotated cartesian frame and only gets the vector that points towards
#the longest side since the vector pointing that way will be at the
#same index as the scale for the smaller side. This is genius!
# - Andrew Knee
#move to first attempt
print("moving in front of dock")
goal_pos = None
curr_pose = yield self.tx_pose
side_a_bool = False
side_b_bool = False
side_a = bbox_enu+position
side_b = -bbox_enu+position
#determine which long side is closer to us, go to the closer one
#(assume VRX peeps are nice and will always have the open side of the dock closer)
if np.linalg.norm(side_a - curr_pose[0]) < np.linalg.norm(side_b - curr_pose[0]):
print("side_a")
goal_pos = side_a
side_a_bool = True
else:
print("side_b")
goal_pos = side_b
side_b_bool = True
yield self.move.set_position(goal_pos).look_at(position).go()
#at this moment, we are directly facing the middle of a long side of the dock
#check if the dock is the correct side.
yield self.nh.sleep(1)
pixel_diff = yield self.dock_checks()
#if we are on the wrong side, try going to other side of dock
if pixel_diff is None:
yield self.move.backward(7).go(blind=True, move_type="skid")
if side_a_bool:
print("switching to side_b")
goal_pos = side_b
side_a_bool = False
side_b_bool = True
if side_b_bool:
print("switching to side_a")
goal_pos = side_a
side_b_bool = False
side_a_bool = True
yield self.move.set_position(goal_pos).look_at(position).go()
yield self.nh.sleep(1)
pixel_diff = yield self.dock_checks()
if pixel_diff is None:
print("Could not find any viable options for docking")
defer.returnValue(None)
#do we see any symbols?
target_symbol = self.color + "_" + self.shape
symbol_position = yield self.get_symbol_position(target_symbol)
#define how far left and right we want to do
rot = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
side_vect = 0.80 * bbox_enu
if side_a_bool:
print("creating side a left and right position")
self.left_position = np.dot(rot, -side_vect) + side_a
self.right_position = np.dot(rot, side_vect) + side_a
self.dock_point_left = np.dot(rot, -side_vect) + position
self.dock_point_right = np.dot(rot, side_vect) + position
else:
print("creating side b left and right position")
self.left_position = np.dot(rot, side_vect) + side_b
self.right_position = np.dot(rot, -side_vect) + side_b
self.dock_point_left = np.dot(rot, side_vect) + position
self.dock_point_right = np.dot(rot, -side_vect) + position
#if there are symbols, do docking procedure by going to corresponding symbol
print("The correct docking location is ", symbol_position)
#position boat in front of correct symbol
if symbol_position == "left":
yield self.move.set_position(self.left_position).look_at(self.dock_point_left).go(blind=True, move_type="skid")
position = self.dock_point_left
elif symbol_position == "right":
yield self.move.set_position(self.right_position).look_at(self.dock_point_right).go(blind=True, move_type="skid")
position = self.dock_point_right
#enter dock
yield self.nh.sleep(1)
if symbol_position == "foggy":
print("It seems to be a little foggy")
yield self.dock_fire_undock(foggy=True)
else:
yield self.dock_fire_undock(foggy=False)
defer.returnValue(True)
def run(self, args):
self.debug_points_pub = self.nh.advertise('/dock_pannel_points',
PointCloud2)
self.bridge = CvBridge()
self.image_debug_pub = self.nh.advertise('/dock_mask_debug', Image)
self.init_front_left_camera()
args = str.split(args, ' ')
self.color = args[0]
self.shape = args[1]
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=False))
info = yield self.front_left_camera_info_sub.get_next_message()
self.camera_model.fromCameraInfo(info)
pcodar_cluster_tol = DoubleParameter()
pcodar_cluster_tol.name = 'cluster_tolerance_m'
pcodar_cluster_tol.value = 20
yield self.pcodar_set_params(doubles=[pcodar_cluster_tol])
self.nh.sleep(5)
pose = yield self.find_dock()
yield self.move.look_at(pose).set_position(pose).backward(20).go()
# get updated points now that we a closer
dock, pose = yield self.get_sorted_objects(name='dock', n=1)
dock = dock[0]
position, quat = pose_to_numpy(dock.pose)
rotation = quaternion_matrix(quat)
bbox = rosmsg_to_numpy(dock.scale)
bbox[2] = 0
min_dim = np.argmin(bbox[:2])
bbox[min_dim] = 0
bbox_enu = np.dot(rotation[:3, :3], bbox)
yield self.move.set_position(
(bbox_enu + position)).look_at(position).go()
curr_color, masked_image = yield self.get_color()
if curr_color != self.color:
# Dock at the other side of the dock, no further perception
yield self.move.backward(5).go()
yield self.move.set_position(
(-bbox_enu + position)).look_at(position).go()
yield self.dock()
else:
if self.get_shape(masked_image) == self.shape:
print 'shapes match'
yield self.dock()
else:
# go to other one
yield self.move.backward(5).go()
yield self.move.set_position(
(-bbox_enu + position)).look_at(position).go()
curr_color, _ = yield self.get_color()
task_info = yield self.task_info_sub.get_next_message()
if curr_color != self.color and task_info.remaining_time.secs >= 60:
# if we got the right color on the other side and the wrong color on this side,
# go dock in the other side
yield self.move.backward(5).go()
yield self.move.set_position(
(bbox_enu + position)).look_at(position).go()
yield self.dock()
else:
# this side is the right color
# Dock at the other side of the dock, no further perception
yield self.dock()
yield self.send_feedback('Done!')
def set_odom(msg):
return setattr(self, 'odom', mil_tools.pose_to_numpy(msg.pose.pose))
def odometrySubscriber(self, msg):
pose = msg.pose.pose
self.pos, self.ori = pose_to_numpy(pose)
self.odomSet = True
def set_odom(msg):
return setattr(self, 'odom',
mil_tools.pose_to_numpy(msg.pose.pose))
def set_odom(self, msg):
self.odom = mil_tools.pose_to_numpy(msg.pose.pose)
self.odom_pub.publish(msg)
def set_odom(self, msg):
self.odom = mil_tools.pose_to_numpy(msg.pose.pose)
self.odom_pub.publish(msg)
