class RosTargetDispatcher:
""" translates ros commands into drone function calls """
# settings
TARGET_RADIUS = 50 #mm
# variables
target = None
state = None
principalplane = None
comorg = None
def __init__(self, drone, allow_ros_control):
# init vars
self.allow_ros_control = allow_ros_control
self.reset()
self.principalplane = Principalplane()
self.comorg = CommandOrganiser(drone)
# ros subscriptions
rospy.Subscriber("/goal", PoseStamped, self.setTarget)
rospy.Subscriber("/ardrone/hybriddata", String, self.updateDirection)
def reset(self, target=None):
self.state = State()
self.target = target
def setTarget(self, poseStamped):
if poseStamped.header.frame_id == '-1':
self.reset()
print " [targetdispatcher] done"
elif self.target != poseStamped.pose:
self.reset(poseStamped.pose)
self.comorg.stop()
def updateDirection(self, string_obj):
""" init & checks """
pos = decode_string_to_dict(string_obj)
if not self.target:
return self.comorg.stop()
if not self.allow_ros_control():
self.reset(self.target)
return self.comorg.stop()
# transform pos to principalplane
pos = self.principalplane.updateAndTransform(pos)
P_quad = array([pos['x'], pos['y']])
psi_q = pos['psi']
""" check confidence & num points """
if pos['confidence'] < 0.6:
print " [targetdispatcher] no confidence"
return self.comorg.stop()
elif pos['num_found_points'] < 100:
print " [targetdispatcher] not enough SLAM points"
return self.comorg.stop()
""" get local target """
# <disable state system>
#if self.state.state == State.IDLE:
# self.state.startpos = P_quad
# self.state.state = State.SET_ANGLE
# P_target = P_quad
#elif self.state.state == State.SET_ANGLE:
# P_target = self.state.startpos
#elif self.state.state == State.FLY_TO_TARGET:
# P_target = array([self.target.position.x, self.target.position.y])
P_target = array([self.target.position.x, self.target.position.y])
dP = P_target - P_quad
""" get target psi """
o = self.target.orientation
target_psi = 180/pi * euler_from_quaternion([o.x, o.y, o.z, o.w])[2]
angle_diff = ((target_psi - psi_q + 180) % 360) - 180
""" check if reached target """
reached_target_pos = norm(dP) < self.TARGET_RADIUS
reached_target_angle = abs(angle_diff) < 5 #deg
""" update state """
if reached_target_angle and reached_target_pos:
# <disable state system>
#if self.state.state == State.SET_ANGLE:
# print "angle ok, proceeding"
# self.state.state = State.FLY_TO_TARGET
#elif self.state.state == State.FLY_TO_TARGET:
# if not self.comorg.stopped: print "target reached"
# self.comorg.stop()
if not self.comorg.stopped: print "target reached"
return self.comorg.stop()
""" update commands """
if not reached_target_pos:
psi = psi_q / 180.0 * pi
u_qx = array([cos(psi), sin(psi)])
u_qy = array([-sin(psi), cos(psi)])
dFront = dot(dP, u_qx)
dRight = -dot(dP, u_qy)
for diff, action in ((dFront, 'moveforward'), (dRight, 'moveright')):
if abs(diff) < self.TARGET_RADIUS/sqrt(2):
continue
else:
self.comorg.schedule(action, diff)
if not reached_target_angle:
self.comorg.schedule('turnright', -angle_diff)
self.comorg.flush()
class PointParser:
slamScaleToMetricScale = None
principalplane = None
def __init__(self, room_num):
""" get slam-to-metric scale """
self.slamScaleToMetricScale = float(
open(mapdir(room_num) + '/slamtometric').readline())
self.slamScaleToMetricScale /= Scale.get(room_num) # apply scaling
""" recover principalplane """
tfstring = open(mapdir(room_num) + '/principalplane').read()
self.principalplane = Principalplane()
self.principalplane.transformation.setFromString(tfstring)
def parse(self, pt, fromstring=False, frompose=False):
""" step 0: convert to point """
if fromstring:
pt = self.stringToArray(pt)
if frompose:
pt = self.lnToTranslation(pt)
""" step 1: convert from PTAM to ROS axes """
pt = array([
pt[1] / self.slamScaleToMetricScale,
-1 * -pt[0] / self.slamScaleToMetricScale, ## TMP mirror around XY
pt[2] / self.slamScaleToMetricScale,
])
""" step 2: transform via principalplane """
pt = self.principalplane.updateAndTransform(pt, update=False)
pt[2] += 200 # TMP (ad hoc)
""" step 3: convert mm to m """
pt /= 1e3
""" step 3: remove if outlier """
if norm(pt) > 20:
return None
return pt
@staticmethod
def stringToArray(string):
return array([float(x) for x in string.strip().split(' ')])
@classmethod
def lnToTranslation(Self, mu):
mu = array(mu)
w = mu[3:]
theta_sq = sum(w * w)
theta = sqrt(theta_sq)
w_x_mu = cross(w, mu[:3])
if theta_sq < 1e-8:
A = 1.0 - theta_sq / 6
B = 0.5
translation = mu[:3] + 0.5 * w_x_mu
else:
if theta_sq < 1e-6:
C = (1.0 - theta_sq / 20) / 6
A = 1.0 - theta_sq * C
B = 0.5 - 0.25 * one_6th * theta_sq
else:
inv_theta = 1.0 / theta
A = sin(theta) * inv_theta
B = (1 - cos(theta)) * (inv_theta * inv_theta)
C = (1 - A) * (inv_theta * inv_theta)
translation = mu[:3] + B * w_x_mu + C * cross(w, w_x_mu)
rotation = Self.rodrigues_so3_exp(w, A, B)
# invert translation
rotinv = rotation.I
translinv = -(translation * rotinv.T)
translinv = array(translinv).squeeze()
return translinv
@staticmethod
def rodrigues_so3_exp(w, A, B):
R = zeros((3, 3))
wx2 = w[0] * w[0]
wy2 = w[1] * w[1]
wz2 = w[2] * w[2]
R[0][0] = 1.0 - B * (wy2 + wz2)
R[1][1] = 1.0 - B * (wx2 + wz2)
R[2][2] = 1.0 - B * (wx2 + wy2)
a = A * w[2]
b = B * (w[0] * w[1])
R[0][1] = b - a
R[1][0] = b + a
a = A * w[1]
b = B * (w[0] * w[2])
R[0][2] = b + a
R[2][0] = b - a
a = A * w[0]
b = B * (w[1] * w[2])
R[1][2] = b - a
R[2][1] = b + a
return matrix(R)
class RosTargetDispatcher:
""" translates ros commands into drone function calls """
# settings
TARGET_RADIUS = 50 #mm
# variables
target = None
state = None
principalplane = None
comorg = None
def __init__(self, drone, allow_ros_control):
# init vars
self.allow_ros_control = allow_ros_control
self.reset()
self.principalplane = Principalplane()
self.comorg = CommandOrganiser(drone)
# ros subscriptions
rospy.Subscriber("/goal", PoseStamped, self.setTarget)
rospy.Subscriber("/ardrone/hybriddata", String, self.updateDirection)
def reset(self, target=None):
self.state = State()
self.target = target
def setTarget(self, poseStamped):
if poseStamped.header.frame_id == '-1':
self.reset()
print " [targetdispatcher] done"
elif self.target != poseStamped.pose:
self.reset(poseStamped.pose)
self.comorg.stop()
def updateDirection(self, string_obj):
""" init & checks """
pos = decode_string_to_dict(string_obj)
if not self.target:
return self.comorg.stop()
if not self.allow_ros_control():
self.reset(self.target)
return self.comorg.stop()
# transform pos to principalplane
pos = self.principalplane.updateAndTransform(pos)
P_quad = array([pos['x'], pos['y']])
psi_q = pos['psi']
""" check confidence & num points """
if pos['confidence'] < 0.6:
print " [targetdispatcher] no confidence"
return self.comorg.stop()
elif pos['num_found_points'] < 100:
print " [targetdispatcher] not enough SLAM points"
return self.comorg.stop()
""" get local target """
# <disable state system>
#if self.state.state == State.IDLE:
# self.state.startpos = P_quad
# self.state.state = State.SET_ANGLE
# P_target = P_quad
#elif self.state.state == State.SET_ANGLE:
# P_target = self.state.startpos
#elif self.state.state == State.FLY_TO_TARGET:
# P_target = array([self.target.position.x, self.target.position.y])
P_target = array([self.target.position.x, self.target.position.y])
dP = P_target - P_quad
""" get target psi """
o = self.target.orientation
target_psi = 180 / pi * euler_from_quaternion([o.x, o.y, o.z, o.w])[2]
angle_diff = ((target_psi - psi_q + 180) % 360) - 180
""" check if reached target """
reached_target_pos = norm(dP) < self.TARGET_RADIUS
reached_target_angle = abs(angle_diff) < 5 #deg
""" update state """
if reached_target_angle and reached_target_pos:
# <disable state system>
#if self.state.state == State.SET_ANGLE:
# print "angle ok, proceeding"
# self.state.state = State.FLY_TO_TARGET
#elif self.state.state == State.FLY_TO_TARGET:
# if not self.comorg.stopped: print "target reached"
# self.comorg.stop()
if not self.comorg.stopped: print "target reached"
return self.comorg.stop()
""" update commands """
if not reached_target_pos:
psi = psi_q / 180.0 * pi
u_qx = array([cos(psi), sin(psi)])
u_qy = array([-sin(psi), cos(psi)])
dFront = dot(dP, u_qx)
dRight = -dot(dP, u_qy)
for diff, action in ((dFront, 'moveforward'), (dRight,
'moveright')):
if abs(diff) < self.TARGET_RADIUS / sqrt(2):
continue
else:
self.comorg.schedule(action, diff)
if not reached_target_angle:
self.comorg.schedule('turnright', -angle_diff)
self.comorg.flush()
