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)
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()