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 __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 __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)
class ExploringController:
# utility vars (static)
targetPublisher = rospy.Publisher('/goal', PoseStamped)
statePublisher = StatePublisher('/explorer/state')
principalplane = Principalplane()
# regular vars
wall_points = None
walls = None
action = None
def __init__(self):
self.walls = []
self.wall_points = []
#self.setAction(FlyToWallAction(self, 45))
self.setAction(CycleAction(self))
def setAction(self, action_object):
self.action = action_object
# output
action_descr = str(action_object) if action_object else "no action"
self.statePublisher.publish(action_descr)
print " [ExploringController] new action:", action_descr
def setNextAction(self):
#TODO
#x.__class__.__name__
self.setAction(None)
rospy.signal_shutdown("done")
def updatePoints(self, points_str):
points_arr = decode_string_to_array(points_str)
points_arr = self.principalplane.updateAndTransform(points_arr)
self.wall_points = [x for x in points_arr if is_wall(x)]
def updatePos(self, pos_str):
pos = decode_string_to_dict(pos_str)
pos = self.principalplane.updateAndTransform(pos)
P = array([pos['x'], pos['y']])
psi = pos['psi']
done = self.action.update(P, psi) if self.action else False
if done:
self.resetTarget()
self.setNextAction()
def publishTarget(self, (xy, psi)):
if xy != None:
p = Point(xy[0], xy[1], 0)
o = Quaternion()
o.x, o.y, o.z, o.w = quaternion_from_euler(0, 0, psi)
target = PoseStamped(pose=Pose(position=p, orientation=o))
else:
target = PoseStamped()
target.header.frame_id = '-1'
self.targetPublisher.publish(target)
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)
from numpy.random import randn
# ROS imports
import roslib
roslib.load_manifest('controller')
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
# custom imports
from utility import *
from floorseparator import is_wall
from principalplane import Principalplane
from wall_detection.walldetectionlib import detect_walls
# globals
publisher = rospy.Publisher('/map/walls', String)
principalplane = Principalplane()
# subscribing function
def process_map_points(string):
""" get points """
rospy.loginfo("Starting calculation...")
points_arr = decode_string_to_array(string)
points_arr = principalplane.updateAndTransform(points_arr)
""" apply wall detection """
L = detect_walls(points_arr)
""" get walls """
walls = [l.E() for l in L]
""" publish walls """
publisher.publish(encode_tuplearray_to_string(walls))
rospy.loginfo("published {0} lines".format(len(L)))
#!/usr/bin/python
""" imports """
# std imports
# ...
# ROS imports
import roslib
roslib.load_manifest('wallviz')
import rospy
from std_msgs.msg import String
# custom imports
from utility import *
from principalplane import Principalplane
""" ros init """
rospy.init_node('point_visualizer')
principalplane = Principalplane()
""" get principalplane params """
principalplane.updateTransformation()
params = principalplane.getParams()
""" save params """
print "params = ", params
open('map/principalplane', 'w').write(params)
""" done """
print "done"
#!/usr/bin/python
""" imports """
# std imports
# ...
# ROS imports
import roslib; roslib.load_manifest('wallviz'); import rospy
from std_msgs.msg import String
# custom imports
from utility import *
from principalplane import Principalplane
""" ros init """
rospy.init_node('point_visualizer')
principalplane = Principalplane()
""" get principalplane params """
principalplane.updateTransformation()
params = principalplane.getParams()
""" save params """
print "params = ", params
open('map/principalplane', 'w').write(params)
""" done """
print "done"
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 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()
