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