def call_with_nodehandle_sim_time(f):
rosmaster = yield start_rosmaster()
try:
nh = yield NodeHandle.from_argv(
'node',
argv=[
'__ip:=127.0.0.1',
'__master:=http://127.0.0.1:%i' % (rosmaster.get_port(), ),
],
anonymous=True,
)
try:
@apply
@util.cancellableInlineCallbacks
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()
try:
yield nh.set_param('/use_sim_time', True)
nh2 = yield NodeHandle.from_argv(
'node2',
argv=[
'__ip:=127.0.0.1',
'__master:=http://127.0.0.1:%i' %
(rosmaster.get_port(), ),
],
anonymous=True,
)
try:
defer.returnValue((yield f(nh2)))
finally:
yield nh2.shutdown()
finally:
clock_thread.cancel()
clock_thread.addErrback(
lambda fail: fail.trap(defer.CancelledError))
finally:
yield nh.shutdown()
finally:
yield rosmaster.stop()
def main(param_prefix, args):
nh = yield NodeHandle.from_argv("on_the_fly_mission_runner",
anonymous=True)
image_sub = yield nh.subscribe(args.topic_name, Image)
img = yield util.wrap_timeout(image_sub.get_next_message(),
5).addErrback(errback)
np_img = image_helpers.get_image_msg(img, "bgr8")
cv2.imshow(args.topic_name, np_img)
t = Thresholder(np_img, 'hsv' if args.hsv else 'bgr')
k = 0
while k != ord('q'): # q to quit without saving
t.update_mask()
k = cv2.waitKey(50) & 0xFF
if k == ord('s'): # s to save parameters
print "Saving params:"
temp = t.to_dict()
print temp
nh.set_param(param_prefix, temp)
break
cv2.destroyAllWindows()
reactor.stop()
def call_with_nodehandle_sim_time(f):
rosmaster = yield start_rosmaster()
try:
nh = yield NodeHandle.from_argv('node',
argv=[
'__ip:=127.0.0.1',
'__master:=http://127.0.0.1:%i' % (rosmaster.get_port(),),
],
anonymous=True,
)
try:
@apply
@util.cancellableInlineCallbacks
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()
try:
yield nh.set_param('/use_sim_time', True)
nh2 = yield NodeHandle.from_argv('node2',
argv=[
'__ip:=127.0.0.1',
'__master:=http://127.0.0.1:%i' % (rosmaster.get_port(),),
],
anonymous=True,
)
try:
defer.returnValue((yield f(nh2)))
finally:
yield nh2.shutdown()
finally:
clock_thread.cancel()
clock_thread.addErrback(lambda fail: fail.trap(defer.CancelledError))
finally:
yield nh.shutdown()
finally:
yield rosmaster.stop()
def _init(self):
self.nh = yield NodeHandle.from_argv("mission_planner")
global nh
nh = self.nh
global n
n = yield Navigator(nh)._init()
self.sub_database = yield nh.subscribe('/database/object_found', PerceptionObject, self.new_item)
# self.servcl_exploration_yield = yield nh.get_service_client("/exploration/yield_control", std_msgs.msg.Bool)
self.refresh()
defer.returnValue(self)
def init_(self, sim_mode=False):
"""Initialize the txros aspects of the MissionPlanner b."""
self.nh = yield NodeHandle.from_argv("mission_planner")
self.navigator = yield Navigator(self.nh)._init(sim_mode)
self.helper = yield DBHelper(self.nh).init_()
yield self.helper.begin_observing(self.new_item)
# This needs to be called in case begin_observing doesn't call refresh
self.refresh()
defer.returnValue(self)
def init_(self, sim_mode=False):
"""Initialize the txros aspects of the MissionPlanner."""
self.nh = yield NodeHandle.from_argv("mission_planner")
self.navigator = yield Navigator(self.nh)._init(sim_mode)
self.helper = yield DBHelper(self.nh).init_()
yield self.helper.begin_observing(self.new_item)
self.pub_msn_info = yield self.nh.advertise("/mission_planner/mission", String)
yield self.nh.sleep(1)
# This needs to be called in case begin_observing doesn't call refresh
self.refresh()
defer.returnValue(self)
def init_(self, yaml_text, sim_mode=False):
"""Initialize the txros aspects of the MissionPlanner."""
self.sim_mode = sim_mode
assert yaml_text is not None, "YOU NEED A YAML TEXT TO RUN A MISSION"
self.nh = yield NodeHandle.from_argv("mission_planner")
self.navigator = yield Navigator(self.nh)._init(sim_mode)
self.missions_left, self.base_mission, self.tree, self.total_time = yield yaml_parse(yaml_text, self.navigator,
self.total_time)
self.pub_msn_info = yield self.nh.advertise("/mission_planner/mission", String)
self.helper = yield DBHelper(self.nh).init_(navigator=self.navigator)
yield self.nh.sleep(1)
defer.returnValue(self)
def call_with_nodehandle(f):
rosmaster = yield start_rosmaster()
try:
nh = yield NodeHandle.from_argv('node',
argv=['__ip:=127.0.0.1',
'__master:=http://127.0.0.1:%i' % (rosmaster.get_port(),), ],
anonymous=True,)
try:
defer.returnValue((yield f(nh)))
finally:
yield nh.shutdown()
finally:
yield rosmaster.stop()
def init_(self, yaml_text, sim_mode=False):
"""Initialize the txros aspects of the MissionPlanner."""
self.sim_mode = sim_mode
assert yaml_text is not None, "YOU NEED A YAML TEXT TO RUN A MISSION"
self.nh = yield NodeHandle.from_argv("mission_planner")
self.navigator = yield Navigator(self.nh)._init(sim_mode)
self.missions_left, self.base_mission, self.tree, self.total_time = yield yaml_parse(
yaml_text, self.navigator, self.total_time)
self.pub_msn_info = yield self.nh.advertise("/mission_planner/mission",
String)
self.helper = yield DBHelper(self.nh).init_(navigator=self.navigator)
yield self.nh.sleep(1)
defer.returnValue(self)
def _init(self):
self.nh = yield NodeHandle.from_argv("object_classifier")
global nh
nh = self.nh
self.pub_obj_found = yield self.nh.advertise('/classifier/object',
PerceptionObject)
self.pub_object_searching = yield self.nh.advertise(
'/classifier/looking_for', Marker)
self.sub_unclassified = yield self.nh.subscribe(
'/unclassified/objects', PerceptionObjects, self.new_objects)
defer.returnValue(self)
def init_(self, yaml_text, sim_mode=False):
self.sim_mode = sim_mode
assert yaml_text is not None, "YOU NEED A YAML TEXT TO RUN A MISSION"
"""Initialize the txros aspects of the MissionPlanner."""
self.total_mission_count = len(yaml_text.keys())
self.nh = yield NodeHandle.from_argv("mission_planner")
self.navigator = yield Navigator(self.nh)._init(sim_mode)
self.pub_msn_info = yield self.nh.advertise("/mission_planner/mission", String)
self.helper = yield DBHelper(self.nh).init_(navigator=self.navigator)
yield self._load_missions(yaml_text)
if self.base_mission is not None:
self.total_mission_count -= 1
yield self.nh.sleep(1)
defer.returnValue(self)
def main():
"""Main method to the test node"""
nh, args = yield NodeHandle.from_argv_with_remaining("navigator_test")
available_missions = [
mission_name for mission_name in dir(navigator_tests)
if mission_name[0] != '_'
]
parser = argparse.ArgumentParser(description='NaviGator Test')
parser.add_argument('tests',
type=str,
nargs='+',
help='Test(s) from the navigator_tests folder to run.')
args = parser.parse_args(args[1:])
if "list" in args.tests:
print "\nAvailable tests:\n *",
print '\n * '.join(available_missions)
print
defer.returnValue(reactor.stop())
for test in args.tests:
# Make sure all missions exist before we run
assert test in available_missions, "'{}' test not found".format(test)
for test in args.tests:
fprint("Running Test!\n", title="TEST")
to_run = getattr(navigator_tests, test)
to_run = _import(to_run)
to_run = to_run(nh)
to_run.create_spoofs()
result = yield to_run.run_tests()
if result is None:
fprint("{} finished with no result.".format(test), title="TEST")
else:
for r in result:
fprint("{} finished with:".format(test), title="TEST")
print r
defer.returnValue(reactor.stop())
def _init(self):
self.nh = yield NodeHandle.from_argv("my_object_database")
global nh
nh = self.nh
self.pub_object_found = yield self.nh.advertise(
'/database/object_found', PerceptionObject)
self.pub_object_markers = yield self.nh.advertise(
'/database/objects_classified', MarkerArray)
self.pub_object_curr = yield self.nh.advertise(
'/database/objects_curr', Marker)
self.serv_single_query = yield self.nh.advertise_service(
'/database/single', ObjectDBSingleQuery, self.query_single)
self.serv_full_query = yield self.nh.advertise_service(
'/database/full', ObjectDBFullQuery, self.query_full)
self.sub_object_classification = yield self.nh.subscribe(
'/classifier/object', PerceptionObject, self.new_object)
defer.returnValue(self)
def main():
"""Main method to the test node"""
nh, args = yield NodeHandle.from_argv_with_remaining("navigator_test")
available_missions = [mission_name for mission_name in dir(navigator_tests) if mission_name[0] != '_']
parser = argparse.ArgumentParser(description='Navigator Test')
parser.add_argument('tests', type=str, nargs='+',
help='Test(s) from the navigator_tests folder to run.')
args = parser.parse_args(args[1:])
if "list" in args.tests:
print "\nAvailable tests:\n *",
print '\n * '.join(available_missions)
print
defer.returnValue(reactor.stop())
for test in args.tests:
# Make sure all missions exist before we run
assert test in available_missions, "'{}' test not found".format(test)
for test in args.tests:
fprint("Running Test!\n", title="TEST")
to_run = getattr(navigator_tests, test)
to_run = _import(to_run)
to_run = to_run(nh)
to_run.create_spoofs()
result = yield to_run.run_tests()
if result is None:
fprint("{} finished with no result.".format(test), title="TEST")
else:
for r in result:
fprint("{} finished with:".format(test), title="TEST")
print r
defer.returnValue(reactor.stop())
def main(param_prefix, args):
nh = yield NodeHandle.from_argv("on_the_fly_mission_runner", anonymous=True)
image_sub = yield nh.subscribe(args.topic_name, Image)
img = yield util.wrap_timeout(image_sub.get_next_message(), 5).addErrback(errback)
np_img = image_helpers.get_image_msg(img, "bgr8")
cv2.imshow(args.topic_name, np_img)
t = Thresholder(np_img, 'hsv' if args.hsv else 'bgr')
k = 0
while k != ord('q'): # q to quit without saving
t.update_mask()
k = cv2.waitKey(50) & 0xFF
if k == ord('s'): # s to save parameters
print "Saving params:"
temp = t.to_dict()
print temp
nh.set_param(param_prefix, temp)
break
cv2.destroyAllWindows()
reactor.stop()
def main():
nh = yield NodeHandle.from_argv("testing")
n = yield _Navigator(nh)._init()
print (yield n.vision_request('sonar'))
def main():
nh = yield NodeHandle.from_argv("navigator_singleton")
n = yield Navigator(nh)._init()
fprint((yield n.vision_proxies['start_gate'].get_response()))
def main():
nh = yield NodeHandle.from_argv("testing")
n = yield _Navigator(nh)._init()
print(yield n.vision_request('sonar'))
def main():
nh = yield NodeHandle.from_argv("testing")
n = yield Navigator(nh)._init()
yield n.vision_proxies['tester'].get_response()
def main():
nh = yield NodeHandle.from_argv("navigator_singleton")
n = yield Navigator(nh)._init()
fprint((yield n.vision_proxies['start_gate'].get_response()))