def update(self, dt, desired, current):
(p, o), (p_dot, o_dot) = current
(desired_p, desired_o), (desired_p_dot, desired_o_dot), (desired_p_dotdot, desired_o_dotdot) = desired
world_from_body = transformations.quaternion_matrix(o)[:3, :3]
x_dot = numpy.concatenate([
world_from_body.dot(p_dot),
world_from_body.dot(o_dot),
])
# world_from_desiredbody = transformations.quaternion_matrix(desired_o)[:3, :3]
desired_x_dot = numpy.concatenate([
world_from_body.dot(desired_p_dot),
world_from_body.dot(desired_o_dot),
])
desired_x_dotdot = numpy.concatenate([
world_from_body.dot(desired_p_dotdot),
world_from_body.dot(desired_o_dotdot),
])
error_position_world = numpy.concatenate([
desired_p - p,
quat_to_rotvec(transformations.quaternion_multiply(
desired_o,
transformations.quaternion_inverse(o),
)),
])
if self.config['two_d_mode']:
error_position_world = error_position_world * [1, 1, 0, 0, 0, 1]
world_from_body2 = numpy.zeros((6, 6))
world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body
# Permitting lambda assignment b/c legacy
body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T) # noqa
error_velocity_world = (desired_x_dot + body_gain(numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot
if self.config['two_d_mode']:
error_velocity_world = error_velocity_world * [1, 1, 0, 0, 0, 1]
pd_output = body_gain(numpy.diag(self.config['ks'])).dot(error_velocity_world)
output = pd_output
if self.config['use_rise']:
rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \
body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world))
self._rise_term = self._rise_term + dt / 2 * (rise_term_int + self._rise_term_int_prev)
self._rise_term_int_prev = rise_term_int
output = output + self._rise_term
else:
# zero rise term so it doesn't wind up over time
self._rise_term = numpy.zeros(6)
self._rise_term_int_prev = numpy.zeros(6)
output = output + body_gain(numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot)
output = output + body_gain(numpy.diag(self.config['vel_feedforward'])).dot(desired_x_dot)
# Permitting lambda assignment b/c legacy
wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]), world_from_body.T.dot(output[3:6])) # noqa
return wrench_from_vec(pd_output), wrench_from_vec(output)
def _odom_update(self):
pos = self.last_odom_msg.pose.pose.position
yaw = quat_to_rotvec(
xyzw_array(self.last_odom_msg.pose.pose.orientation))[2]
vel = self.last_odom_msg.twist.twist.linear
dYaw = self.last_odom_msg.twist.twist.angular.z
self._odom_x_label.setText('X: %3.3f' % pos.x)
self._odom_y_label.setText('Y: %3.3f' % pos.y)
self._odom_yaw_label.setText('Yaw: %3.3f' % yaw)
self._odom_d_x_label.setText('dX: %3.3f' % vel.x)
self._odom_d_y_label.setText('dY: %3.3f' % vel.y)
self._odom_d_yaw_label.setText('dYaw: %3.3f' % dYaw)
def main(nh, entrance='x', exit='1'):
boat = yield boat_scripting.get_boat(nh)
try:
boat.switch_path_planner('a_star_rpp')
while True:
gates = yield boat.get_gates()
if len(gates) == 0:
print 'No gates detected'
continue
current_boat_pos = boat.odom.position
yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi
# Zip up numpy positions in enu - boat_pos
gate_pos = [
xyz_array(g.position) - current_boat_pos for g in gates
]
gates = zip(gate_pos, gates)
# Filter out yaw
gate = None
if len(gates) > 0:
gate = min(gates, key=lambda x: np.linalg.norm(x[0]))
else:
gate = gates[0]
#print gate
# Translate back to enu
gate_pos = gate[0] + current_boat_pos
gate_orientation = gate[1].yaw + np.pi / 2
if abs(gate_orientation - yaw) < np.pi / 2:
yield move_on_line.main(nh, gate_pos,
np.array([0, 0, gate_orientation]))
else:
yield move_on_line.main(nh,
gate_pos,
np.array([0, 0, gate_orientation]),
flip=True)
finally:
boat.default_state()
def main(nh, entrance='x', exit='1'):
boat = yield boat_scripting.get_boat(nh)
try:
boat.switch_path_planner('a_star_rpp')
while True:
gates = yield boat.get_gates()
if len(gates) == 0:
print 'No gates detected'
continue
current_boat_pos = boat.odom.position
yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi
# Zip up numpy positions in enu - boat_pos
gate_pos = [xyz_array(g.position) - current_boat_pos for g in gates]
gates = zip(gate_pos, gates)
# Filter out yaw
gate = None
if len(gates) > 0:
gate = min(gates, key = lambda x: np.linalg.norm(x[0]))
else:
gate = gates[0]
#print gate
# Translate back to enu
gate_pos = gate[0] + current_boat_pos
gate_orientation = gate[1].yaw + np.pi/2
if abs(gate_orientation - yaw) < np.pi / 2:
yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]))
else:
yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]), flip=True)
finally:
boat.default_state()
def orient(boat):
angle_to_move = 0
final_cloud = []
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
pointcloud_base = []
# loops until lidar gets a good filtered lidar reading
while len(final_cloud) <= 0:
pointcloud = yield boat.get_pointcloud()
yield util.sleep(0.1) # sleep to avoid tooPast errors
pointcloud_base = yield boat.to_baselink(pointcloud)
yield util.sleep(0.1) # sleep to avoid tooPast errors
pointcloud_enu = []
# Append pointcloud data to enu
for p in pc2.read_points(pointcloud, field_names=("x", "y", "z"), skip_nans=False, uvs=[]):
pointcloud_enu.append((p[0], p[1], p[2]))
yield util.sleep(0.1) # sleep to avoid tooPast errors
final_cloud = []
# Filter lidar data to only data right in front of the boat
# Since both pointcloud have the sane index for the same points,
# we then use the baselink pointcloud data to fiter enu points in front of the boat
print "Filtering lidar data"
for i in range(0, len(pointcloud_base)):
temp = pointcloud_base[i]
if abs(temp[1]) < 0.3:
final_cloud.append(pointcloud_enu[i])
# Use final point cloud to get x and y mean to be used in line generation
for i in final_cloud:
x_mean += i[0]
y_mean += i[1]
# Find x-mean and y-mean
y_mean = y_mean / len(final_cloud)
x_mean = x_mean / len(final_cloud)
for i in final_cloud:
numerator += (i[0] - x_mean) * (i[1] - y_mean)
denom += (i[0] - x_mean) * (i[0] - x_mean)
if denom == 0:
denom = 1
m = numerator / denom
b = y_mean - (m * x_mean)
x1 = 1
x2 = 100
# Vector we want to be parallel to
parallel_vector = m * 1 + b
# Gets heading of the boat as unit vector
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
# Vector to be parrallel to
vec1 = numpy.array([x1, parallel_vector, 0])
# Vector of the boat
vec2 = numpy.array([(heading[0]), (heading[1]), 0])
angle_to_move = 2
move = 0
angle_to_move = 1
# Finds angle between boat unit vector and dock
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
# Angle betwen the boat and the shore
angle_to_move = math.acos(cosine)
# Scan lidar to get the distances from the shore to the boat for use in creating triangle
distances = yield boat.get_distance_from_object(0.05)
# Caluculate distance to move to be directly in front of point
theda_one = 0.9 - angle_to_move
L1 = distances[0]
distance_to_move = math.sin(theda_one) * L1
if vec2[0] < 0:
# print "Yawing left ", angle_to_move
print "Moving right ", distance_to_move
# boat.move.turn_left(angle_to_move).go()
left = boat.move.left(-distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
if vec2[0] > 0:
# print "Yawing right ", angle_to_move
print "Moving left ", distance_to_move
# boat.move.turn_left(angle_to_move).go()
left = boat.move.left(distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
def main(nh):
boat = yield boat_scripting.get_boat(nh)
boat_x = boat.odom.position[0]
boat_y = boat.odom.position[1]
closest = None
summation = 100
closest_buoy = 0
buoys = yield boat.get_bouys()
while closest is None:
while len(buoys.buoys) <= 0:
buoys = yield boat.get_bouys()
for i in buoys.buoys:
x_dif = boat_x - i.position.x
y_dif = boat_y - i.position.y
temp_sum = math.sqrt(x_dif ** 2 + y_dif ** 2)
if temp_sum > 10:
# Don't go more than 10 meters
continue
#print temp_sum
if temp_sum < summation:
summation = temp_sum
closest = i
x_move = boat_x - closest.position.x
y_move = boat_y - closest.position.y
point = numpy.array([closest.position.x, closest.position.y, 0])
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
vec2 = numpy.array([heading[0], heading[1]])
vec1 = numpy.array([closest.position.x, closest.position.y])
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
# Angle betwen the boat and the shore
angle_to_move = math.atan(cosine)
print angle_to_move
boat.move.yaw_left(-angle_to_move).go()
yield boat.move.set_position(point).go()
yield boat.move.turn_left_deg(-20).go()
for i in xrange(4):
yield boat.move.forward(3).go()
yield boat.move.turn_left_deg(90).go()
def update(self, dt, desired, current):
((desired_p, desired_o), (desired_p_dot, desired_o_dot),
(desired_p_dotdot,
desired_o_dotdot)), ((p, o), (p_dot, o_dot)) = desired, current
world_from_body = transformations.quaternion_matrix(o)[:3, :3]
x_dot = numpy.concatenate([
world_from_body.dot(p_dot),
world_from_body.dot(o_dot),
])
world_from_desiredbody = transformations.quaternion_matrix(
desired_o)[:3, :3]
desired_x_dot = numpy.concatenate([
world_from_body.dot(desired_p_dot),
world_from_body.dot(desired_o_dot),
])
desired_x_dotdot = numpy.concatenate([
world_from_body.dot(desired_p_dotdot),
world_from_body.dot(desired_o_dotdot),
])
error_position_world = numpy.concatenate([
desired_p - p,
quat_to_rotvec(
transformations.quaternion_multiply(
desired_o,
transformations.quaternion_inverse(o),
)),
])
if self.config['two_d_mode']:
error_position_world = error_position_world * [1, 1, 0, 0, 0, 1]
world_from_body2 = numpy.zeros((6, 6))
world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body
body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T)
error_velocity_world = (desired_x_dot + body_gain(
numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot
if self.config['two_d_mode']:
error_velocity_world = error_velocity_world * [1, 1, 0, 0, 0, 1]
pd_output = body_gain(numpy.diag(
self.config['ks'])).dot(error_velocity_world)
output = pd_output
if self.config['use_rise']:
rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \
body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world))
self._rise_term = self._rise_term + dt / 2 * (
rise_term_int + self._rise_term_int_prev)
self._rise_term_int_prev = rise_term_int
output = output + self._rise_term
else:
# zero rise term so it doesn't wind up over time
self._rise_term = numpy.zeros(6)
self._rise_term_int_prev = numpy.zeros(6)
output = output + body_gain(
numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot)
output = output + body_gain(numpy.diag(
self.config['vel_feedforward'])).dot(desired_x_dot)
wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]),
world_from_body.T.dot(output[3:6]))
return wrench_from_vec(pd_output), wrench_from_vec(output)
def orientation_from_pose(p):
return quat_to_rotvec(xyzw_array(p.orientation))
def main(nh):
while not rospy.is_shutdown():
boat = yield boat_scripting.get_boat(nh)
#boat.pan_lidar(min_angle=2.7, max_angle=3.14, freq=.75)
#boat.float_on()
#gate_viz_pub = rospy.Publisher('gates_viz', Marker, queue_size = 10)
#rospy.init_node("sdfhjhsdfjhgsdf")
angle_to_move = 0
hold = []
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
pointcloud_base = []
# loops until lidar gets a good filtered lidar reading
while len(hold) <= 0:
pointcloud = yield boat.get_pointcloud()
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_base = yield boat.to_baselink(pointcloud)
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_enu = []
for p in pc2.read_points(pointcloud, field_names=("x", "y", "z"), skip_nans=False, uvs=[]):
pointcloud_enu.append((p[0], p[1], p[2]))
yield util.sleep(.1) # sleep to avoid tooPast errors
point_in_base = []
hold = []
# Filter lidar data to only data right in front of the boat
print "Filtering lidar data"
for i in range(0, len(pointcloud_base)):
temp = pointcloud_base[i]
if abs(temp[1]) < .3:
hold.append(pointcloud_enu[i])
for i in hold:
x_mean += i[0]
y_mean += i[1]
y_mean = y_mean/len(hold)
x_mean = x_mean/len(hold)
for i in hold:
numerator += (i[0] - x_mean)*(i[1] - y_mean)
denom += (i[0] - x_mean)*(i[0] - x_mean)
if denom == 0: denom = 1
m = numerator/denom
b = y_mean - (m * x_mean)
x1 = 1
x2 = 100
par_vect = m*1 + b
point2 = m*x2 + b
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
# Line we want to be orthagonal to
position1 = to_vector(x1, par_vect)
position3 = to_vector((heading[0]) + boat.odom.position[0], (heading[1]) + boat.odom.position[1])
position4 = to_vector((heading[0]+ .01) + boat.odom.position[0], (heading[1]+.1) + boat.odom.position[1])
#Vector to be parrallel to
vec1 = numpy.array([x1, par_vect, 0])
vec2 = ([0,0,0])
angle_to_move = 2
move = 0
angle_to_move = 1
#def find_angle():
# Calculte angle_to_move and position to move to get in front of object
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
vec2 = numpy.array([(heading[0]), (heading[1]), 0])
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
angle_to_move = math.acos(cosine)
#find_angle()
distances = yield boat.get_distance_from_object(.05)
theda_one = .9 - angle_to_move
L1 = distances[0]
distance_to_move = math.sin(theda_one) * L1
veccc = to_vector(x1,par_vect)
vecc = to_vector(x2, point2)
'''
m = Marker()
m.header = Header(frame_id = '/enu', stamp = rospy.Time.now())
m.ns = 'gates'
m.id = 1
m.type = Marker.LINE_STRIP
m.action = Marker.ADD
m.scale.x = 0.1
m.color.r = 0.5
m.color.b = 0.75
m.color.g = 0.1
m.color.a = 1.0
m.points.append(veccc)
m.points.append(vecc)
gate_viz_pub.publish(m)
'''
if vec2[0] < 0:
print "Yawing left ", angle_to_move
print "Moving right ", distance_to_move
raw_input("Press enter to continue...")
boat.move.turn_left(angle_to_move).go()
yield boat.move.left(-distance_to_move-2).go()
if vec2[0] > 0:
print "Yawing right ", angle_to_move
print "Moving left ", distance_to_move
raw_input("Press enter to continue...")
boat.move.turn_left(angle_to_move).go()
yield boat.move.left(distance_to_move-2).go()
def main(nh):
# #Print mission start msg
#print 'Finding start gate with laser'
boat = yield boat_scripting.get_boat(nh)
try:
yaw = quat_to_rotvec(boat.odom.orientation)[2]
#print 'Pan the lidar between the maximum angle and slightly above horizontal'
boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)
# How many gates we've gone through
gates_passed = 0
have_gate = False
last_gate_pos = None
move = None
while gates_passed < 2:
#print 'Get gates'
gates = yield boat.get_gates()
(gate_pos, gate_orientation) = (None, None)
if gates_passed == 0:
(gate_pos,
gate_orientation) = filter_gates(boat, gates, yaw, False)
else:
(gate_pos,
gate_orientation) = filter_gates(boat, gates, yaw, True)
# Check if valid gate found
if gate_pos is not None:
have_gate = True
# Check if we previously found a gate
if last_gate_pos is not None:
# Check if the gate center has drifted
# Don't go to a different gate (dis < 5)
dis = numpy.linalg.norm(last_gate_pos - gate_pos)
#print 'Distance: ', dis
if dis >= 0.1 and dis < 3:
print 'Gate drifted re-placing goal point'
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
-GATE_DISTANCE)
else:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
GATE_DISTANCE)
else:
#print 'Still happy with my current goal'
pass
else:
# Just found a gate
print 'Moving to gate ' + str(gates_passed + 1)
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
#yield boat.move.yaw_left_deg(30).go()
move = move_on_line.main(nh, gate_pos,
gate_orientation,
-GATE_DISTANCE)
print 'zzz'
yield util.sleep(1.0)
else:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
GATE_DISTANCE)
# Set last gate pos
last_gate_pos = gate_pos
else:
if have_gate is False:
print 'No gate found moving forward 1m'
yield boat.move.forward(1).go()
else:
print 'Lost sight of gate; Continuing to last known position'
# Check if task complete
if have_gate and move.called:
print 'Move complete'
#yield boat.move.forward(3).go()
yaw = quat_to_rotvec(boat.odom.orientation)[2]
have_gate = False
last_gate_pos = None
gates_passed = gates_passed + 1
finally:
boat.default_state()
def main_loop(self, event):
self.lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2 * math.pi) - math.pi
# sub pd-controller sans rise
rot = None
# Get tf from /enu to /base_link
# Since we are dealing with differences and not absolute positions not translation is required
try:
(_,
rot) = self.tf_listener.lookupTransform('/base_link', '/enu',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn('Tf exception: ' + str(e))
if rot is not None:
# Convert to euler angle ( we only care about rotation about z)
theta = quat_to_rotvec(numpy.array(rot))[2]
# Difference in /enu frame
to_desired_state = self.desired_state[0:2] - self.state[0:2]
# Convert to base_link
s = numpy.sin(theta)
c = numpy.cos(theta)
rot_matrix_enu_base = numpy.array([[c, -s], [s, c]])
to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
to_desired_state = numpy.insert(to_desired_state, 2,
0) # Append a zero for z
# Note: Angular differences are the same in base_link as enu so no tf required
e = numpy.concatenate([
to_desired_state,
map(smallest_coterminal_angle,
self.desired_state[3:6] - self.state[3:6])
]) # e_1 in paper
# Convert desired state dot into base_link
# angular velocities do not rquire transformation as they are differences (rendering them frameless)
# To convert velocites from enu + desired_orientation to base link:
# transform to enu: rotate the velocity vector by desired_orientation
# transform to base link: rotate the velocity vector by the angle between enu and base_link
vels = self.desired_state_dot[0:2]
theta2 = self.desired_state[5]
s = numpy.sin(theta2)
c = numpy.cos(theta2)
rot_matrix_desired_enu = numpy.array([[c, -s], [s, c]])
vels = rot_matrix_desired_enu.dot(vels)
vels = rot_matrix_enu_base.dot(vels)
vels = numpy.insert(vels, 2, 0)
desired_state_dot = numpy.concatenate(
[vels, self.desired_state_dot[3:6]])
#print 'Desired_state tf: ', desired_state_dot
e_dot = desired_state_dot - self.state_dot
output = self.K_p.dot(e) + self.K_d.dot(e_dot)
# Apply simple moving average filter
new = output / self.num_to_avg
old = (self.outputs[0] / self.num_to_avg)
self.last_average = self.last_average - old + new
self.outputs.popleft()
self.outputs.append(output)
# Resuse output var
#print 'Outputs: ', self.outputs
output = self.last_average
#print 'Last Average: ', output
self.lock.release()
self.x_error = e[0]
self.y_error = e[1]
self.z_error = e[5]
self.dx_error = e_dot[0]
self.dy_error = e_dot[1]
self.dz_error = e_dot[5]
#self.to_terminal()
if (not (self.odom_active)):
output = [0, 0, 0, 0, 0, 0]
if (self.enable & self.killed == False):
self.controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=output[0],
y=output[1],
z=0),
torque=Vector3(x=0, y=0, z=output[5]),
)))
if ((self.x_error < 1) & (self.y_error < 1) &
(self.z_error < 1)):
self.waypoint_progress.publish(True)
if (self.killed == True):
rospy.logwarn('PD_Controller KILLED: %s' %
self.kill_listener.get_kills())
self.controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=0, y=0, z=0),
torque=Vector3(x=0, y=0, z=0),
)))
else:
self.lock.release()
def main(nh):
# #Print mission start msg
#print 'Finding start gate with laser'
boat = yield boat_scripting.get_boat(nh)
try:
yaw = quat_to_rotvec(boat.odom.orientation)[2]
#print 'Pan the lidar between the maximum angle and slightly above horizontal'
boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)
# How many gates we've gone through
gates_passed = 0
have_gate = False
last_gate_pos = None
move = None
while gates_passed < 2:
#print 'Get gates'
gates = yield boat.get_gates()
(gate_pos, gate_orientation) = (None, None)
if gates_passed == 0:
(gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, False)
else:
(gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, True)
# Check if valid gate found
if gate_pos is not None:
have_gate = True
# Check if we previously found a gate
if last_gate_pos is not None:
# Check if the gate center has drifted
# Don't go to a different gate (dis < 5)
dis = numpy.linalg.norm(last_gate_pos - gate_pos)
#print 'Distance: ', dis
if dis >= 0.1 and dis < 3:
print 'Gate drifted re-placing goal point'
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
else:
move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)
else:
#print 'Still happy with my current goal'
pass
else:
# Just found a gate
print 'Moving to gate ' + str(gates_passed + 1)
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
#yield boat.move.yaw_left_deg(30).go()
move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
print 'zzz'
yield util.sleep(1.0)
else:
move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)
# Set last gate pos
last_gate_pos = gate_pos
else:
if have_gate is False:
print 'No gate found moving forward 1m'
yield boat.move.forward(1).go()
else:
print 'Lost sight of gate; Continuing to last known position'
# Check if task complete
if have_gate and move.called:
print 'Move complete'
#yield boat.move.forward(3).go()
yaw = quat_to_rotvec(boat.odom.orientation)[2]
have_gate = False
last_gate_pos = None
gates_passed = gates_passed + 1
finally:
boat.default_state()
def orient(boat):
angle_to_move = 0
final_cloud = []
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
pointcloud_base = []
# loops until lidar gets a good filtered lidar reading
while len(final_cloud) <= 0:
pointcloud = yield boat.get_pointcloud()
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_base = yield boat.to_baselink(pointcloud)
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_enu = []
# Append pointcloud data to enu
for p in pc2.read_points(pointcloud,
field_names=("x", "y", "z"),
skip_nans=False,
uvs=[]):
pointcloud_enu.append((p[0], p[1], p[2]))
yield util.sleep(.1) # sleep to avoid tooPast errors
final_cloud = []
# Filter lidar data to only data right in front of the boat
# Since both pointcloud have the sane index for the same points,
# we then use the baselink pointcloud data to fiter enu points in front of the boat
print "Filtering lidar data"
for i in range(0, len(pointcloud_base)):
temp = pointcloud_base[i]
if abs(temp[1]) < .3:
final_cloud.append(pointcloud_enu[i])
# Use final point cloud to get x and y mean to be used in line generation
for i in final_cloud:
x_mean += i[0]
y_mean += i[1]
# Find x-mean and y-mean
y_mean = y_mean / len(final_cloud)
x_mean = x_mean / len(final_cloud)
for i in final_cloud:
numerator += (i[0] - x_mean) * (i[1] - y_mean)
denom += (i[0] - x_mean) * (i[0] - x_mean)
if denom == 0: denom = 1
m = numerator / denom
b = y_mean - (m * x_mean)
x1 = 1
x2 = 100
# Vector we want to be parallel to
parallel_vector = m * 1 + b
# Gets heading of the boat as unit vector
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
# Vector to be parrallel to
vec1 = numpy.array([x1, parallel_vector, 0])
# Vector of the boat
vec2 = numpy.array([(heading[0]), (heading[1]), 0])
angle_to_move = 2
move = 0
angle_to_move = 1
# Finds angle between boat unit vector and dock
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
# Angle betwen the boat and the shore
angle_to_move = math.acos(cosine)
# Scan lidar to get the distances from the shore to the boat for use in creating triangle
distances = yield boat.get_distance_from_object(.05)
# Caluculate distance to move to be directly in front of point
theda_one = .9 - angle_to_move
L1 = distances[0]
distance_to_move = math.sin(theda_one) * L1
if vec2[0] < 0:
#print "Yawing left ", angle_to_move
print "Moving right ", distance_to_move
#boat.move.turn_left(angle_to_move).go()
left = boat.move.left(-distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
if vec2[0] > 0:
#print "Yawing right ", angle_to_move
print "Moving left ", distance_to_move
#boat.move.turn_left(angle_to_move).go()
left = boat.move.left(distance_to_move - 2).go()
forward = boat.move.forward(distance_to_move - 3).go()
yield left, forward
def main_loop(self, event):
self.lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2*math.pi) - math.pi
# sub pd-controller sans rise
rot = None
# Get tf from /enu to /base_link
# Since we are dealing with differences and not absolute positions not translation is required
try:
(_, rot) = self.tf_listener.lookupTransform('/base_link', '/enu', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn('Tf exception: ' + str(e))
if rot is not None:
# Convert to euler angle ( we only care about rotation about z)
theta = quat_to_rotvec(numpy.array(rot))[2]
# Difference in /enu frame
to_desired_state = self.desired_state[0:2] - self.state[0:2]
# Convert to base_link
s = numpy.sin(theta)
c = numpy.cos(theta)
rot_matrix_enu_base = numpy.array([[c, -s],
[s, c]])
to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
to_desired_state = numpy.insert(to_desired_state, 2, 0) # Append a zero for z
# Note: Angular differences are the same in base_link as enu so no tf required
e = numpy.concatenate([to_desired_state, map(smallest_coterminal_angle, self.desired_state[3:6] - self.state[3:6])]) # e_1 in paper
# Convert desired state dot into base_link
# angular velocities do not rquire transformation as they are differences (rendering them frameless)
# To convert velocites from enu + desired_orientation to base link:
# transform to enu: rotate the velocity vector by desired_orientation
# transform to base link: rotate the velocity vector by the angle between enu and base_link
vels = self.desired_state_dot[0:2]
theta2 = self.desired_state[5]
s = numpy.sin(theta2)
c = numpy.cos(theta2)
rot_matrix_desired_enu = numpy.array([[c, -s],
[s, c]])
vels = rot_matrix_desired_enu.dot(vels)
vels = rot_matrix_enu_base.dot(vels)
vels = numpy.insert(vels, 2, 0)
desired_state_dot = numpy.concatenate([vels, self.desired_state_dot[3:6]])
#print 'Desired_state tf: ', desired_state_dot
e_dot = desired_state_dot - self.state_dot
output = self.K_p.dot(e) + self.K_d.dot(e_dot)
# Apply simple moving average filter
new = output / self.num_to_avg
old = (self.outputs[0] / self.num_to_avg)
self.last_average = self.last_average - old + new
self.outputs.popleft()
self.outputs.append(output)
# Resuse output var
#print 'Outputs: ', self.outputs
output = self.last_average
#print 'Last Average: ', output
self.lock.release()
self.x_error = e[0]
self.y_error = e[1]
self.z_error = e[5]
self.dx_error = e_dot[0]
self.dy_error = e_dot[1]
self.dz_error = e_dot[5]
#self.to_terminal()
if (not(self.odom_active)):
output = [0,0,0,0,0,0]
if (self.enable & self.killed==False):
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= output[0],y= output[1],z= 0),
torque = Vector3(x=0,y= 0,z= output[5]),
))
)
if((self.x_error < 1) & (self.y_error < 1) & (self.z_error < 1)):
self.waypoint_progress.publish(True)
if (self.killed == True):
rospy.logwarn('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= 0,y= 0,z= 0),
torque = Vector3(x=0,y= 0,z= 0),
))
)
else:
self.lock.release()
def orientation_from_pose(p): return quat_to_rotvec(xyzw_array(p.orientation))
