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 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 _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 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 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 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 callback(req):
yield util.wall_sleep(.5)
defer.returnValue(TwoIntsResponse(sum=req.a + req.b))
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()
def _kill_soon():
yield util.wall_sleep(0)
os._exit(0)
def f():
yield util.wall_sleep(3)
defer.returnValue('retval')
