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