def poller(nh): db_srv = nh.get_service_client('/database/requests', ObjectDBQuery) while True: yield util.wall_sleep(3) while True: try: db_res = yield db_srv( ObjectDBQueryRequest( name='shooter', cmd='', )) except: traceback.print_exc() yield util.wall_sleep(1) else: break if not db_res.found: print 'shooter not found' continue obj = db_res.objects[0] points = map(msg_helpers.ros_to_np_3D, obj.points) if points: center_holder[0] = numpy.mean(points, axis=0)
def poller(nh): db_srv = nh.get_service_client('/database/requests', ObjectDBQuery) while True: yield util.wall_sleep(3) while True: try: db_res = yield db_srv(ObjectDBQueryRequest( name='shooter', cmd='', )) except BaseException: traceback.print_exc() yield util.wall_sleep(1) else: break if not db_res.found: print 'shooter not found' continue obj = db_res.objects[0] points = map(msg_helpers.ros_to_np_3D, obj.points) if points: center_holder[0] = numpy.mean(points, axis=0)
def wait_for_server(self): while not (set(self._goal_pub.get_connections()) & set(self._cancel_pub.get_connections()) & set(self._status_sub.get_connections()) & set(self._result_sub.get_connections()) & set(self._feedback_sub.get_connections())): yield util.wall_sleep(0.1) # XXX bad bad bad
def start_rosmaster(): tmpd = tempfile.mkdtemp() try: logfile = '%s/master.log' % (tmpd,) p = subprocess.Popen(['rosmaster', '--core', '-p', '0', '__log:=' + logfile]) for i in xrange(1000): if os.path.exists(logfile): success = False with open(logfile, 'rb') as f: for line in f: if ': Master initialized: port[0], uri[http://' in line: port = int(line.split(':')[-1].split('/')[0]) success = True break if success: break yield util.wall_sleep(.01) else: assert False, 'rosmaster never came up' class ROSMaster(object): def get_port(self): return port def stop(self): p.terminate() return threads.deferToThread(p.wait) defer.returnValue(ROSMaster()) finally: shutil.rmtree(tmpd)
def start_rosmaster(): tmpd = tempfile.mkdtemp() try: logfile = '%s/master.log' % (tmpd, ) p = subprocess.Popen( ['rosmaster', '--core', '-p', '0', '__log:=' + logfile]) for i in xrange(1000): if os.path.exists(logfile): success = False with open(logfile, 'rb') as f: for line in f: if ': Master initialized: port[0], uri[http://' in line: port = int(line.split(':')[-1].split('/')[0]) success = True break if success: break yield util.wall_sleep(.01) else: assert False, 'rosmaster never came up' class ROSMaster(object): def get_port(self): return port def stop(self): p.terminate() return threads.deferToThread(p.wait) defer.returnValue(ROSMaster()) finally: shutil.rmtree(tmpd)
def wait_for_service(self): while True: try: yield self._node_handle._master_proxy.lookupService(self._name) except rosxmlrpc.Error: yield util.wall_sleep(.1) # XXX bad continue else: return
def clock_thread(): try: clock_pub = nh.advertise('/clock', Clock) t = genpy.Time.from_sec(12345) while True: clock_pub.publish(Clock(clock=t, )) yield util.wall_sleep(.01) t = t + genpy.Duration.from_sec(.1) except Exception: traceback.print_exc()
def run(sub): # buoy_search is a Deferred print "We're looking for a buoy" response = yield sub.buoy.get_pose('red') if not response.found: print 'failed to discover buoy location' return back = sub.move.forward(0.1) transform = yield sub._tf_listener.get_transform( '/map', '/stereo_front', response.pose.header.stamp ) tft = tf.Transform.from_Pose_message(response.pose.pose) full_transform = transform * tft print full_transform._p # yield sub.move.set_position(full_transform._p).go() print 'setting height' if full_transform._p[2] > -0.2: print 'Detected buoy above the water' return yield sub.move.height(full_transform._p[2]).go(speed=0.2) yield util.wall_sleep(0.2) print 'looking at' yield sub.move.look_at(full_transform._p).go(speed=0.1) print 'position' yield util.wall_sleep(0.2) # go = clip_norm(transform._p, 0.01, ) dist = response.pose.pose.position.z print 'd', dist if dist > 1.0: print 'moving forward' yield sub.move.forward(dist - 1.0).go(speed=0.2) # yield sub.move.forward(0.2).go(speed=0.1) # yield util.wall_sleep(0.1) yield back.go(speed=0.1) # yield sub.move.forward( print "Bumped the buoy"
def _real_shutdown(self): yield util.wall_sleep(0) # ensure that we are called directly from reactor rather than from a callback, avoiding strange case where a cancellableInlineCallbacks is cancelled while it's running self._is_running = False while self._shutdown_callbacks: self._shutdown_callbacks, old = set(), self._shutdown_callbacks old_dfs = [func() for func in old] for df in old_dfs: try: yield df except: traceback.print_exc()
def sleep_until(self, time): if isinstance(time, (float, int)): time = genpy.Time.from_sec(time) elif not isinstance(time, genpy.Time): raise TypeError('expected float or genpy.Time') if self._use_sim_time: while self._sim_time < time: yield self._clock_sub.get_next_message() else: while True: current_time = self.get_time() if current_time >= time: return yield util.wall_sleep((time - current_time).to_sec())
def clock_thread(): try: clock_pub = nh.advertise('/clock', Clock) t = genpy.Time.from_sec(12345) while True: clock_pub.publish(Clock( clock=t, )) yield util.wall_sleep(.01) t = t + genpy.Duration.from_sec(.1) except Exception: traceback.print_exc()
def _publisher_thread(self, url): while True: try: proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(url), self._node_handle._name) value = yield proxy.requestTopic(self._name, [['TCPROS']]) protocol, host, port = value conn = yield endpoints.TCP4ClientEndpoint( reactor, host, port).connect( util.AutoServerFactory(lambda addr: tcpros.Protocol())) try: conn.sendString( tcpros.serialize_dict( dict( message_definition=self._type._full_text, callerid=self._node_handle._name, topic=self._name, md5sum=self._type._md5sum, type=self._type._type, ))) header = tcpros.deserialize_dict((yield conn.receiveString())) self._connections[conn] = header.get('callerid', None) try: while True: data = yield conn.receiveString() msg = self._type().deserialize(data) try: self._callback(msg) except: traceback.print_exc() self._last_message = msg self._last_message_time = self._node_handle.get_time( ) old, self._message_dfs = self._message_dfs, [] for df in old: df.callback(msg) finally: del self._connections[conn] finally: conn.transport.loseConnection() except (error.ConnectionDone, error.ConnectionLost, error.ConnectionRefusedError): pass except Exception: traceback.print_exc() yield util.wall_sleep( 1 ) # pause so that we don't repeatedly reconnect immediately on failure
def run(sub): # buoy_search is a Deferred print "We're looking for a buoy" response = yield sub.buoy.get_pose('red') if not response.found: print 'failed to discover buoy location' return back = sub.move.forward(0.1) transform = yield sub._tf_listener.get_transform( '/map', '/stereo_front', response.pose.header.stamp) tft = tf.Transform.from_Pose_message(response.pose.pose) full_transform = transform * tft print full_transform._p # yield sub.move.set_position(full_transform._p).go() print 'setting height' if full_transform._p[2] > -0.2: print 'Detected buoy above the water' return yield sub.move.height(full_transform._p[2]).go(speed=0.2) yield util.wall_sleep(0.2) print 'looking at' yield sub.move.look_at(full_transform._p).go(speed=0.1) print 'position' yield util.wall_sleep(0.2) # go = clip_norm(transform._p, 0.01, ) dist = response.pose.pose.position.z print 'd', dist if dist > 1.0: print 'moving forward' yield sub.move.forward(dist - 1.0).go(speed=0.2) # yield sub.move.forward(0.2).go(speed=0.1) # yield util.wall_sleep(0.1) yield back.go(speed=0.1) # yield sub.move.forward( print "Bumped the buoy"
def _publisher_thread(self, url): while True: try: proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(url), self._node_handle._name) value = yield proxy.requestTopic(self._name, [['TCPROS']]) protocol, host, port = value conn = yield endpoints.TCP4ClientEndpoint(reactor, host, port).connect( util.AutoServerFactory(lambda addr: tcpros.Protocol())) try: conn.sendString(tcpros.serialize_dict(dict( message_definition=self._type._full_text, callerid=self._node_handle._name, topic=self._name, md5sum=self._type._md5sum, type=self._type._type, ))) header = tcpros.deserialize_dict((yield conn.receiveString())) self._connections[conn] = header.get('callerid', None) try: while True: data = yield conn.receiveString() msg = self._type().deserialize(data) try: self._callback(msg) except: traceback.print_exc() self._last_message = msg self._last_message_time = self._node_handle.get_time() old, self._message_dfs = self._message_dfs, [] for df in old: df.callback(msg) finally: del self._connections[conn] finally: conn.transport.loseConnection() except (error.ConnectionDone, error.ConnectionLost, error.ConnectionRefusedError): pass except Exception: traceback.print_exc() yield util.wall_sleep(1) # pause so that we don't repeatedly reconnect immediately on failure
def run(sub): # buoy_search is a Deferred print "We're looking for a buoy" print "Executing search pattern" yield sub.move.right(2.0).go() yield sub.move.down(0.3).go() print "Now trying to pose it" response = yield sub.buoy.get_pose('red') if not response.found: print 'failed to discover buoy location' return else: print "Got buoy pose" print response.pose back = sub.move.forward(0.1) full_transform = tf.Transform.from_Pose_message(response.pose.pose) print 'setting height' if full_transform._p[2] > -0.2: print 'Detected buoy above the water' defer.returnValue(False) yield sub.move.depth(-full_transform._p[2]).go(speed=0.2) yield util.wall_sleep(0.2) print 'looking at' yield sub.move.look_at_without_pitching(full_transform._p).go() yield util.wall_sleep(0.2) yield sub.move.look_at_without_pitching(full_transform._p).go() yield util.wall_sleep(0.2) yield sub.move.look_at_without_pitching(full_transform._p).go() yield util.wall_sleep(0.2) dist = np.inf while(dist > 1.0): dist = np.linalg.norm(full_transform._p - sub.pose.position) * 0.5 yield sub.move.look_at_without_pitching(full_transform._p).go() yield util.wall_sleep(0.2) yield sub.move.forward(dist).go() yield util.wall_sleep(0.5) # yield util.wall_sleep(0.1) yield back.go(speed=0.1) # yield sub.move.forward( print "Bumped the buoy"
def callback(req): yield util.wall_sleep(.5) defer.returnValue(TwoIntsResponse(sum=req.a + req.b))
def __new__(cls, ns, name, addr, master_uri, remappings): # constraints: anything blocking here should print something if it's # taking a long time in order to avoid confusion self = object.__new__(cls) if ns: assert ns[0] == '/' assert not ns.endswith('/') self._ns = ns # valid values: '', '/a', '/a/b' assert '/' not in name self._name = self._ns + '/' + name self._shutdown_callbacks = set() reactor.addSystemEventTrigger('before', 'shutdown', self.shutdown) self._addr = addr self._master_uri = master_uri self._remappings = remappings self._master_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(self._master_uri), self._name) self._is_running = True self._xmlrpc_handlers = {} self._xmlrpc_server = reactor.listenTCP( 0, server.Site(_XMLRPCSlave(self))) self._shutdown_callbacks.add(self._xmlrpc_server.loseConnection) self._xmlrpc_server_uri = 'http://%s:%i/' % ( self._addr, self._xmlrpc_server.getHost().port) self._tcpros_handlers = {} @util.cancellableInlineCallbacks def _handle_tcpros_conn(conn): try: header = tcpros.deserialize_dict((yield conn.receiveString())) def default(header, conn): conn.sendString( tcpros.serialize_dict( dict(error='unhandled connection'))) conn.transport.loseConnection() if 'service' in header: self._tcpros_handlers.get(('service', header['service']), default)(header, conn) elif 'topic' in header: self._tcpros_handlers.get(('topic', header['topic']), default)(header, conn) else: conn.sendString( tcpros.serialize_dict( dict(error='no topic or service name detected'))) conn.transport.loseConnection() except: conn.transport.loseConnection() raise def _make_tcpros_protocol(addr): conn = tcpros.Protocol() _handle_tcpros_conn(conn) return conn self._tcpros_server = reactor.listenTCP( 0, util.AutoServerFactory(_make_tcpros_protocol)) self._shutdown_callbacks.add(self._tcpros_server.loseConnection) self._tcpros_server_uri = 'rosrpc://%s:%i' % ( self._addr, self._tcpros_server.getHost().port) self._tcpros_server_addr = self._addr, self._tcpros_server.getHost( ).port while True: try: other_node_uri = yield self._master_proxy.lookupNode( self._name) except rosxmlrpc.Error: break # assume that error means unknown node except Exception: traceback.print_exc() yield util.wall_sleep(1) # pause so we don't retry immediately else: other_node_proxy = rosxmlrpc.Proxy( xmlrpc.Proxy(other_node_uri), self._name) try: yield util.wrap_timeout( other_node_proxy.shutdown( 'new node registered with same name'), 3) except error.ConnectionRefusedError: pass except Exception: traceback.print_exc() break try: self._use_sim_time = yield self.get_param('/use_sim_time') except rosxmlrpc.Error: # assume that error means not found self._use_sim_time = False if self._use_sim_time: def got_clock(msg): self._sim_time = msg.clock self._clock_sub = self.subscribe('/clock', Clock, got_clock) # make sure self._sim_time gets set before we continue yield util.wrap_time_notice(self._clock_sub.get_next_message(), 1, 'getting simulated time from /clock') for k, v in self._remappings.iteritems(): if k.startswith('_') and not k.startswith('__'): yield self.set_param(self.resolve_name('~' + k[1:]), yaml.load(v)) self.advertise_service('~get_loggers', GetLoggers, lambda req: GetLoggersResponse()) self.advertise_service('~set_logger_level', SetLoggerLevel, lambda req: SetLoggerLevelResponse()) defer.returnValue(self)
def __new__(cls, ns, name, addr, master_uri, remappings): # constraints: anything blocking here should print something if it's # taking a long time in order to avoid confusion self = object.__new__(cls) if ns: assert ns[0] == '/' assert not ns.endswith('/') self._ns = ns # valid values: '', '/a', '/a/b' assert '/' not in name self._name = self._ns + '/' + name self._shutdown_callbacks = set() reactor.addSystemEventTrigger('before', 'shutdown', self.shutdown) self._addr = addr self._master_uri = master_uri self._remappings = remappings self._master_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(self._master_uri), self._name) self._is_running = True self._xmlrpc_handlers = {} self._xmlrpc_server = reactor.listenTCP(0, server.Site(_XMLRPCSlave(self))) self._shutdown_callbacks.add(self._xmlrpc_server.loseConnection) self._xmlrpc_server_uri = 'http://%s:%i/' % (self._addr, self._xmlrpc_server.getHost().port) self._tcpros_handlers = {} @util.cancellableInlineCallbacks def _handle_tcpros_conn(conn): try: header = tcpros.deserialize_dict((yield conn.receiveString())) def default(header, conn): conn.sendString(tcpros.serialize_dict(dict(error='unhandled connection'))) conn.transport.loseConnection() if 'service' in header: self._tcpros_handlers.get(('service', header['service']), default)(header, conn) elif 'topic' in header: self._tcpros_handlers.get(('topic', header['topic']), default)(header, conn) else: conn.sendString(tcpros.serialize_dict(dict(error='no topic or service name detected'))) conn.transport.loseConnection() except: conn.transport.loseConnection() raise def _make_tcpros_protocol(addr): conn = tcpros.Protocol() _handle_tcpros_conn(conn) return conn self._tcpros_server = reactor.listenTCP(0, util.AutoServerFactory(_make_tcpros_protocol)) self._shutdown_callbacks.add(self._tcpros_server.loseConnection) self._tcpros_server_uri = 'rosrpc://%s:%i' % (self._addr, self._tcpros_server.getHost().port) self._tcpros_server_addr = self._addr, self._tcpros_server.getHost().port while True: try: other_node_uri = yield self._master_proxy.lookupNode(self._name) except rosxmlrpc.Error: break # assume that error means unknown node except Exception: traceback.print_exc() yield util.wall_sleep(1) # pause so we don't retry immediately else: other_node_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(other_node_uri), self._name) try: yield util.wrap_timeout(other_node_proxy.shutdown('new node registered with same name'), 3) except error.ConnectionRefusedError: pass except Exception: traceback.print_exc() break try: self._use_sim_time = yield self.get_param('/use_sim_time') except rosxmlrpc.Error: # assume that error means not found self._use_sim_time = False if self._use_sim_time: def got_clock(msg): self._sim_time = msg.clock self._clock_sub = self.subscribe('/clock', Clock, got_clock) # make sure self._sim_time gets set before we continue yield util.wrap_time_notice(self._clock_sub.get_next_message(), 1, 'getting simulated time from /clock') for k, v in self._remappings.iteritems(): if k.startswith('_') and not k.startswith('__'): yield self.set_param(self.resolve_name('~' + k[1:]), yaml.load(v)) self.advertise_service('~get_loggers', GetLoggers, lambda req: GetLoggersResponse()) self.advertise_service('~set_logger_level', SetLoggerLevel, lambda req: SetLoggerLevelResponse()) defer.returnValue(self)
def f(): yield util.wall_sleep(3) defer.returnValue('retval')
def test_wall_sleep(self): t1 = reactor.seconds() yield util.wall_sleep(2.) t2 = reactor.seconds() assert 1 <= t2 - t1 <= 3
def _kill_soon(): yield util.wall_sleep(0) os._exit(0)
def main(): try: center = None nh = yield txros.NodeHandle.from_argv('shooter_finder', anonymous=True) pc_sub = nh.subscribe('/velodyne_points', PointCloud2) tf_listener = tf.TransformListener(nh) marker_pub = nh.advertise('/shooter_fit', MarkerArray) result_pub = nh.advertise('/shooter_pose', PoseStamped) odom_sub = nh.subscribe('/odom', Odometry) poller(nh) while True: ts = [time.time()] yield util.wall_sleep(.1) ts.append(time.time()) center = center_holder[0] odom = yield odom_sub.get_next_message() cloud = yield pc_sub.get_next_message() if center is None: yield util.wall_sleep(1) continue #print center Z_MIN = 0 RADIUS = 5 ts.append(time.time()) print 'start' print cloud.header.stamp try: transform = yield tf_listener.get_transform( '/enu', cloud.header.frame_id, cloud.header.stamp) except tf.TooPastError: print 'TooPastError!' continue gen = numpy.array( list( pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")))).T print gen.shape print transform._p gen = (transform._q_mat.dot(gen) + numpy.vstack([transform._p] * gen.shape[1]).T).T good_points = numpy.array([ (x[0], x[1]) for x in gen if x[2] > Z_MIN and math.hypot(x[0] - center[0], x[1] - center[1]) < RADIUS ]) print 'end' if len(good_points) < 10: print 'no good points', len(good_points) continue #if len(good_points) > 100: # good_points = numpy.array(random.sample(list(good_points), 100)) #print good_points ts.append(time.time()) rect = cv2.minAreaRect(good_points.astype(numpy.float32)) rect = numpy.array(rect[0] + rect[1] + (math.radians(rect[2]), )) ts.append(time.time()) #rect = yield threads.deferToThread(fit, good_points, rect) if rect is None: print 'bad fit' continue rect = rect[:2], rect[2:4], rect[4] print 'rect:', rect ts.append(time.time()) marker_pub.publish( MarkerArray(markers=[ Marker( header=Header( frame_id='/enu', stamp=nh.get_time(), ), ns='shooter_ns', id=1, type=Marker.CUBE, action=Marker.ADD, pose=Pose( position=Point(rect[0][0], rect[0][1], .5), orientation=Quaternion( *transformations.quaternion_about_axis( rect[2], [0, 0, 1])), ), scale=Vector3(rect[1][0], rect[1][1], 2), color=ColorRGBA(1, 1, 1, .5), ) ], )) possibilities = [] for i in xrange(4): angle = rect[2] + i / 4 * 2 * math.pi normal = numpy.array([math.cos(angle), math.sin(angle)]) p = numpy.array(rect[0]) + (rect[1][0] if i % 2 == 0 else rect[1][1]) / 2 * normal possibilities.append( (normal, p, transformations.quaternion_about_axis(angle, [0, 0, 1]), rect[1][0] if i % 2 == 1 else rect[1][1])) best = max(possibilities, key=lambda (normal, p, q, size): (msg_helpers.ros_to_np_3D(odom.pose.pose.position)[:2] - rect[0]).dot(normal)) print 'side length:', best[-1] if abs(best[-1] - 1.8) > .25: print 'filtered out based on side length' continue front_points = [ p for p in good_points if abs((p - best[1]).dot(best[0])) < .2 ] print 'len(front_points)', len(front_points) a, b = numpy.linalg.lstsq( numpy.vstack([ [p[0] for p in front_points], [p[1] for p in front_points], ]).T, numpy.ones(len(front_points)), )[0] m, b_ = numpy.linalg.lstsq( numpy.vstack([ [p[0] for p in front_points], numpy.ones(len(front_points)), ]).T, [p[1] for p in front_points], )[0] print 'a, b', a, b, m, b_ print 'RES', numpy.std( [numpy.array([a, b]).dot(p) for p in good_points]) # x = a, b # x . p = 1 # |x| (||x|| . p) = 1 # ||x|| . p = 1 / |x| normal = numpy.array([a, b]) dist = 1 / numpy.linalg.norm(normal) normal = normal / numpy.linalg.norm(normal) p = dist * normal print 'ZZZ', p.dot(numpy.array([a, b])) perp = numpy.array([normal[1], -normal[0]]) x = (best[1] - p).dot(perp) p = p + x * perp if normal.dot(best[0]) < 0: normal = -normal q = transformations.quaternion_about_axis( math.atan2(normal[1], normal[0]), [0, 0, 1]) print 'XXX', p, best[1] #sqrt(a2 + b2) (a x + b y) = 1 result_pub.publish( PoseStamped( header=Header( frame_id='/enu', stamp=nh.get_time(), ), pose=Pose( position=Point(p[0], p[1], 0), orientation=Quaternion(*q), ), )) ts.append(time.time()) print 'timing', ts[-1] - ts[0], map(operator.sub, ts[1:], ts[:-1]) except: traceback.print_exc()
def main(): try: center = None nh = yield txros.NodeHandle.from_argv('shooter_finder', anonymous=True) pc_sub = nh.subscribe('/velodyne_points', PointCloud2) tf_listener = tf.TransformListener(nh) marker_pub = nh.advertise('/shooter_fit', MarkerArray) result_pub = nh.advertise('/shooter_pose', PoseStamped) odom_sub = nh.subscribe('/odom', Odometry) poller(nh) while True: ts = [time.time()] yield util.wall_sleep(.1) ts.append(time.time()) center = center_holder[0] odom = yield odom_sub.get_next_message() cloud = yield pc_sub.get_next_message() if center is None: yield util.wall_sleep(1) continue # print center Z_MIN = 0 RADIUS = 5 ts.append(time.time()) print 'start' print cloud.header.stamp try: transform = yield tf_listener.get_transform('/enu', cloud.header.frame_id, cloud.header.stamp) except tf.TooPastError: print 'TooPastError!' continue gen = numpy.array(list(pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")))).T print gen.shape print transform._p gen = (transform._q_mat.dot(gen) + numpy.vstack([transform._p] * gen.shape[1]).T).T good_points = numpy.array([(x[0], x[1]) for x in gen if x[2] > Z_MIN and math.hypot(x[0] - center[0], x[1] - center[1]) < RADIUS]) print 'end' if len(good_points) < 10: print 'no good points', len(good_points) continue # if len(good_points) > 100: # good_points = numpy.array(random.sample(list(good_points), 100)) # print good_points ts.append(time.time()) rect = cv2.minAreaRect(good_points.astype(numpy.float32)) rect = numpy.array(rect[0] + rect[1] + (math.radians(rect[2]),)) ts.append(time.time()) # rect = yield threads.deferToThread(fit, good_points, rect) if rect is None: print 'bad fit' continue rect = rect[:2], rect[2:4], rect[4] print 'rect:', rect ts.append(time.time()) marker_pub.publish(MarkerArray( markers=[Marker( header=Header( frame_id='/enu', stamp=nh.get_time(), ), ns='shooter_ns', id=1, type=Marker.CUBE, action=Marker.ADD, pose=Pose( position=Point(rect[0][0], rect[0][1], .5), orientation=Quaternion(*transformations.quaternion_about_axis(rect[2], [0, 0, 1])), ), scale=Vector3(rect[1][0], rect[1][1], 2), color=ColorRGBA(1, 1, 1, .5), )], )) possibilities = [] for i in xrange(4): angle = rect[2] + i / 4 * 2 * math.pi normal = numpy.array([math.cos(angle), math.sin(angle)]) p = numpy.array(rect[0]) + (rect[1][0] if i % 2 == 0 else rect[1][1]) / 2 * normal possibilities.append((normal, p, transformations.quaternion_about_axis( angle, [0, 0, 1]), rect[1][0] if i % 2 == 1 else rect[1][1])) best = max(possibilities, key=lambda normal_p_q_size: ( msg_helpers.ros_to_np_3D(odom.pose.pose.position)[:2] - rect[0]).dot(normal_p_q_size[0])) print 'side length:', best[-1] if abs(best[-1] - 1.8) > .25: print 'filtered out based on side length' continue front_points = [pnt for pnt in good_points if abs((pnt - best[1]).dot(best[0])) < .2] print 'len(front_points)', len(front_points) a, b = numpy.linalg.lstsq( numpy.vstack([ [pnt[0] for pnt in front_points], [pnt[1] for pnt in front_points], ]).T, numpy.ones(len(front_points)), )[0] m, b_ = numpy.linalg.lstsq( numpy.vstack([ [pnt[0] for pnt in front_points], numpy.ones(len(front_points)), ]).T, [pnt[1] for pnt in front_points], )[0] print 'a, b', a, b, m, b_ print 'RES', numpy.std([numpy.array([a, b]).dot(pnt) for pnt in good_points]) # x = a, b # x . p = 1 # |x| (||x|| . p) = 1 # ||x|| . p = 1 / |x| normal = numpy.array([a, b]) dist = 1 / numpy.linalg.norm(normal) normal = normal / numpy.linalg.norm(normal) p = dist * normal print 'ZZZ', p.dot(numpy.array([a, b])) perp = numpy.array([normal[1], -normal[0]]) x = (best[1] - p).dot(perp) p = p + x * perp if normal.dot(best[0]) < 0: normal = -normal q = transformations.quaternion_about_axis(math.atan2(normal[1], normal[0]), [0, 0, 1]) print 'XXX', p, best[1] # sqrt(a2 + b2) (a x + b y) = 1 result_pub.publish(PoseStamped( header=Header( frame_id='/enu', stamp=nh.get_time(), ), pose=Pose( position=Point(p[0], p[1], 0), orientation=Quaternion(*q), ), )) ts.append(time.time()) print 'timing', ts[-1] - ts[0], map(operator.sub, ts[1:], ts[:-1]) except BaseException: traceback.print_exc()