help="The ip address or hostname of the host to connect to and receive \
missions from")
parser.add_argument("--mission_port",
type=int,
default=9998,
help="The TCP port to connect to the mission server")
args, _ = parser.parse_known_args()
# Start the application
more_jsons = "{},{}".format(args.map_json, args.robot_json)
app = Application("apps/carter/carter.app.json", more_jsons=more_jsons)
# Enable mission control, if it is allowed
if args.mission_robot_name:
# Load the mission subgraph and set the config based on the input parameters
app.load("packages/behavior_tree/apps/missions.graph.json")
app.nodes["tcp_client"].components["JsonTcpClient"].config[
"host"] = args.mission_host
app.nodes["tcp_client"].components["JsonTcpClient"].config[
"port"] = args.mission_port
app.nodes["mission_control"].components["NodeGroup"].config["node_names"] = \
["goals.goal_behavior"]
app.nodes["robot_name"].components["JsonMockup"].config["json_mock"] = \
{"text":args.mission_robot_name}
app.nodes["goals.goal_behavior"].config[
"disable_automatic_start"] = True
# Send the navigation output back through the json tcp client
app.connect(app.nodes["navigation.subgraph"].components["interface"],
"feedback",
app.nodes["tcp_client"].components["JsonTcpClient"],
"feedback")
from isaac import Application, Message
import argparse
import random
import sys
if __name__ == '__main__':
# Loads message local definition and those from Isaac
Message.load_message_definitions(
"external/com_nvidia_isaac_sdk/messages/*.capnp")
Message.load_message_definitions("apps/foo.capnp")
# Sample of using loaded message definitions
send_msg = Message.create_message_builder('PingProto')
foo_msg = Message.create_message_builder('FooProto')
bar_msg = Message.create_message_builder('BarProto')
# Loads an JSON graph from Isaac repo and runs it
app = Application(name="sub")
app.home_workspace_name = 'velodyne'
app.load(
"@com_nvidia_isaac_sdk//packages/navsim/apps/navsim_tcp.subgraph.json",
prefix="sub")
app.load_module("@com_nvidia_isaac_sdk//packages/sight")
comp_sight = app.nodes["websight"]["WebsightServer"]
comp_sight.config[
"webroot"] = "external/com_nvidia_isaac_sdk/packages/sight/webroot"
comp_sight.config["assetroot"] = "../isaac_assets"
comp_sight.config["port"] = 3000
app.run()
help=
"The ip address or hostname of the host to connect to and receive missions from",
default="localhost")
parser.add_argument("--mission_port",
help="The TCP port to connect to the mission server",
type=int,
default=9998)
args = parser.parse_args()
# Create and start the app
more_jsons = args.map_json
more_jsons += ",packages/navsim/robots/str4.json"
app = Application(
app_filename="packages/cart_delivery/apps/cart_delivery.app.json",
more_jsons=more_jsons)
app.load("packages/cart_delivery/apps/navigation.config.json",
prefix=args.demo_subgraph_name)
app.load(
"packages/cart_delivery/apps/detection_pose_estimation.config.json",
prefix=args.demo_subgraph_name)
if args.pose2_planner:
app.load("packages/cart_delivery/apps/pose2_planner.config.json",
prefix=args.demo_subgraph_name)
if args.mission_robot_name:
# Load the mission subgraph and set the config based on the input parameters
app.load("packages/behavior_tree/apps/missions.graph.json")
app.nodes["tcp_client"].components["JsonTcpClient"].config[
"host"] = args.mission_host
app.nodes["tcp_client"].components["JsonTcpClient"].config[
"port"] = args.mission_port
app.nodes["mission_control"].components["NodeGroup"].config["node_names"] = \
[args.demo_subgraph_name + ".delivery_mission"]
class TestApplication(unittest.TestCase):
'''
Test loading subgraph via the application API
'''
@classmethod
def setUpClass(cls):
# method will be ran once before any test is ran
pass
@classmethod
def tearDownClass(cls):
# method will be ran once after all tests have run
pass
def setUp(self):
# ran before each test
return super().setUp()
def tearDown(self):
# ran after each test
return super().tearDown()
def test_get_component(self):
self._app = Application()
self._app.add('node')
node = self._app.nodes['node']
self.assertIsNotNone(node)
# Get None if no component has such name
component = node['ledger']
self.assertIsNone(component)
component = node['MessageLedger']
self.assertIsNotNone(component)
self._app.start()
self._app.stop()
def test_load_subgraph(self):
self._app = Application()
self._app.add('node')
# loads subgraph and checks if the node/component are created, config is good
self._app.load('packages/pyalice/tests/application_test.subgraph.json',
'foo')
node = self._app.nodes['foo.camera_viewer']
component = node['ImageViewer']
self.assertIsNotNone(component)
fps_readback = self._app.nodes['foo.camera_viewer'].components[
'ImageViewer'].config['target_fps']
self.assertEqual(fps_readback, 11)
camera_name_readback = self._app.nodes['foo.camera_viewer'].components[
'ImageViewer'].config['camera_name']
self.assertEqual(camera_name_readback, 'bar')
# loads the subgraph again with different prefix name
self._app.load('packages/pyalice/tests/application_test.subgraph.json',
'foo1')
node = self._app.nodes['foo1.camera_viewer']
component = node['ImageViewer']
self.assertIsNotNone(component)
fps_readback = self._app.nodes['foo1.camera_viewer'].components[
'ImageViewer'].config['target_fps']
self.assertEqual(fps_readback, 11)
camera_name_readback = self._app.nodes[
'foo1.camera_viewer'].components['ImageViewer'].config[
'camera_name']
self._app.start()
self._app.stop()
def test_load_bogus_subgraph(self):
bogus_json_filename = ''
with tempfile.NamedTemporaryFile('w') as f:
f.write('this is bogus json')
bogus_json_filename = f.name
self._app = Application()
with self.assertRaises(ValueError):
self._app.load('/no/such/file')
with self.assertRaises(ValueError):
self._app.load(bogus_json_filename)
def test_load_subgraph_in_asset_path(self):
self._app = Application()
self._app.add('node')
# override the asset path to point to our test location
self._app._asset_path = 'packages/pyalice/tests/mock_asset_path'
# loads subgraph and checks if the node/component are created, config is good
self._app.load('mock_installed_application_test.subgraph.json', 'foo')
pynode = self._app.nodes['foo.camera_viewer']
self.assertIsNotNone(pynode)
component = pynode['ImageViewer']
self.assertIsNotNone(component)
fps_readback = self._app.nodes['foo.camera_viewer'].components[
'ImageViewer'].config['target_fps']
self.assertEqual(fps_readback, 11)
camera_name_readback = self._app.nodes['foo.camera_viewer'].components[
'ImageViewer'].config['camera_name']
self.assertEqual(camera_name_readback, 'bar')
self._app.start()
self._app.stop()
def test_module_explorer(self):
self._app = Application()
self._app.load_module('json')
self._app.registry.isaac.json.JsonToProto
with self.assertRaises(RuntimeError):
self._app.registry.isaac.foo
def test_node_accesor(self):
self._app = Application()
self._app.load_module('json')
node = self._app.add('foo1')
self.assertIsNotNone(node)
component = node.add(self._app.registry.isaac.json.JsonToProto, 'bar1')
self.assertIsNotNone(component)
node = self._app.add('foo2')
self.assertIsNotNone(node)
component = node.add(self._app.registry.isaac.json.ProtoToJson, 'bar2')
self.assertIsNotNone(component)
self.assertIsNotNone(self._app.nodes['foo1'])
self.assertIsNotNone(self._app.nodes['foo2'])
self.assertIsNone(self._app.nodes['foo3'])
self._app.start()
self._app.stop()
def test_load_module(self):
self._app = Application()
result = self._app.load_module('message_generators')
self.assertTrue(result)
component = self._app.registry.isaac.message_generators.ImageLoader
self.assertIsNotNone(component)
self.assertEqual(str(component),
"['isaac::message_generators::ImageLoader']")
def test_clock(self):
self._app = Application()
self._app.start()
clock = self._app.clock
self.assertIsNotNone(clock)
self.assertIsNotNone(clock.time)
cur_time = clock.time
self.assertGreater(cur_time, 0.0)
self.assertGreater(clock.time, cur_time)
self._app.stop()
def test_pose(self):
self._app = Application()
self._app.start()
self.assertTrue(
self._app.atlas.set_pose(
'foo', 'bar', 1.0,
[np.quaternion(1.0, 0.0, 0.0, 0.0),
np.array([0.0, 0.0, 0.0])]))
self.assertTrue(
self._app.atlas.set_pose(
'foo', 'bar', 2.0,
[np.quaternion(1.0, 0.0, 0.0, 0.0),
np.array([1.0, 2.0, 3.0])]))
read_pose = self._app.atlas.pose('foo', 'bar', 1.5)
self.assertIsNotNone(read_pose)
self.assertEqual(len(read_pose), 2)
q = read_pose[0]
t = read_pose[1]
self.assertLess(q.w - 1.0, 1e-6)
self.assertLess(q.x - 0.0, 1e-6)
self.assertLess(q.y - 0.0, 1e-6)
self.assertLess(q.z - 0.0, 1e-6)
self.assertLess(t[0] - 0.5, 1e-6)
self.assertLess(t[1] - 1.0, 1e-6)
self.assertLess(t[2] - 1.5, 1e-6)
self._app.stop()
def test_node_start_stop(self):
self._app = Application()
result = self._app.load_module('message_generators')
self.assertTrue(result)
node = self._app.add('src')
self.assertIsNotNone(node)
component = node.add(
self._app.registry.isaac.message_generators.PanTiltStateGenerator,
'pantilt')
component.config['tick_period'] = '20 Hz'
self.assertIsNotNone(component)
node_sink = self._app.add('sink')
self.assertIsNotNone(node_sink)
self._app.connect(component, 'target',
node_sink.components['MessageLedger'], 'rcv')
self._app.start()
time.sleep(0.1)
msg = self._app.receive('sink', 'MessageLedger', 'rcv')
self.assertIsNotNone(msg)
node.stop()
msg = self._app.receive('sink', 'MessageLedger', 'rcv')
time.sleep(0.1)
msg = self._app.receive('sink', 'MessageLedger', 'rcv')
self.assertIsNone(msg)
time.sleep(0.1)
msg = self._app.receive('sink', 'MessageLedger', 'rcv')
self.assertIsNone(msg)
node.start()
time.sleep(0.1)
msg = self._app.receive('sink', 'MessageLedger', 'rcv')
self.assertIsNotNone(msg)
self._app.stop()
with self.assertRaises(RuntimeError):
self._app.stop()
def test_wait_for_node(self):
self._app = Application()
self._app.load_module('behavior_tree')
self._app.add('node').add(
self._app.registry.isaac.behavior_tree.TimerBehavior)
self._app.start()
status = self._app.wait_for_node('node')
self._app.stop()
self.assertEqual(str(status), 'Status.Success',
'Should reach Status.Success in 1 second.')
def test_wait_for_node_timeout(self):
self._app = Application()
self._app.load_module('behavior_tree')
self._app.add('node').add(
self._app.registry.isaac.behavior_tree.TimerBehavior)
self._app.start()
status = self._app.wait_for_node('node', duration=0.1)
self._app.stop()
self.assertEqual(str(status), 'Status.Running',
'Should still be in Status.Running')
def test_run_until_succeed(self):
now_secs = time.time()
self._app = Application()
self._app.add('success')
node_success = self._app.nodes['success']
self.assertIsNotNone(node_success)
self._app.nodes['success'].add(SucceedLaterCodelet)
self._app.run('success')
delta_secs = time.time() - now_secs
self.assertGreater(delta_secs, 0.4)
def test_run_until_failure(self):
now_secs = time.time()
self._app = Application()
self._app.add('failure')
node_failure = self._app.nodes['failure']
self.assertIsNotNone(node_failure)
self._app.nodes['failure'].add(FailureLaterCodelet)
self._app.run('failure')
delta_secs = time.time() - now_secs
self.assertGreater(delta_secs, 0.4)
def test_perf_report(self):
PERF_REPORT_PATH = '/tmp/perf_report'
if os.path.exists(PERF_REPORT_PATH):
os.remove(PERF_REPORT_PATH)
self._app = Application(
argv=['--performance_report_out', PERF_REPORT_PATH])
self._app.load_module('behavior_tree')
self._app.add('node').add(
self._app.registry.isaac.behavior_tree.TimerBehavior)
self._app.run(0.2)
self.assertTrue(os.path.exists(PERF_REPORT_PATH))
def test_max_duration(self):
time_now = time.time()
self._app = Application(argv=['--max_duration', '0.5s'])
self._app.load_module('behavior_tree')
self._app.add('node').add(
self._app.registry.isaac.behavior_tree.TimerBehavior)
self._app.run(2.0)
time_dt = time.time() - time_now
self.assertLess(time_dt, 1.0)
time_now = time.time()
self._app = Application(argv=['--max_duration', '0.5s'])
self._app.load_module('behavior_tree')
self._app.add('node').add(
self._app.registry.isaac.behavior_tree.TimerBehavior)
self._app.run(2)
time_dt = time.time() - time_now
self.assertLess(time_dt, 1.0)
def test_print_node(self):
self._app = Application()
self._app.add('foo')
node_names = self._app.nodes._names
self.assertTrue(isinstance(node_names, list) and 'foo' in node_names)
self._app.add('bar')
node_names = self._app.nodes._names
self.assertTrue(
isinstance(node_names, list) and 'foo' in node_names
and 'bar' in node_names)
foo_node = None
bar_node = None
for n in self._app.nodes:
if n.name == 'foo':
foo_node = n
if n.name == 'bar':
bar_node = n
self.assertIsNotNone(foo_node)
self.assertIsNotNone(bar_node)
def test_expand_asset(self):
self._app = Application()
ws, path = self._app._app.expand_asset_path('@workspace_//foo/bar')
self.assertEqual(path, 'external/workspace_/foo/bar')
self.assertEqual(ws, 'workspace_')
ws1 = self._app.home_workspace_name
self.assertEqual(len(ws1), 0)
self._app.home_workspace_name = 'workspace_'
ws2 = self._app.home_workspace_name
self.assertEqual(ws2, 'workspace_')
ws3, path3 = self._app._app.expand_asset_path('@workspace_//foo/bar')
self.assertEqual(ws3, 'workspace_')
self.assertEqual(path3, 'foo/bar')
"--robot_json",
help="The path to the robot json to load into simulation",
default="packages/navsim/robots/carter_stereo.json")
parser.add_argument(
"--more",
help="A comma separated list of additional json files to load")
args = parser.parse_args()
# Create the app and load the required subgraphs
app = Application(name="sim_svio")
app.load_module("atlas")
app.load_module('behavior_tree')
app.load_module('sight')
app.load_module('utils')
app.load_module("viewers")
app.load("packages/navsim/apps/navsim_navigation.subgraph.json",
"simulation")
app.load(
"packages/navigation/apps/differential_base_imu_odometry.subgraph.json",
"odometry")
app.load(
"packages/navigation/apps/differential_base_commander.subgraph.json",
"commander")
app.load(
"packages/visual_slam/apps/stereo_visual_odometry_rgb.subgraph.json",
"svo")
simulation_interface = app.nodes['simulation.interface']
simulation_output = simulation_interface["output"]
# connect the simulation with the odometry and commander
odometry_interface = app.nodes['odometry.subgraph'].components['interface']
args.arm)
joints = []
with open(kinematic_file, 'r') as fd:
kt = json.load(fd)
for link in kt['links']:
if 'motor' in link and link['motor']['type'] != 'constant':
joints.append(link['name'])
# create composite atlas
create_composite_atlas(args.cask, args.arm, joints)
# Create and start the app
app = Application(name="Shuffle Box")
# load bebavior subgraph. this contains the sequency behavior to move the arm between
# waypoints and control suction on/off
app.load("apps/samples/manipulation/shuffle_box_behavior.subgraph.json",
prefix="behavior")
behavior_interface = app.nodes["behavior.interface"]["subgraph"]
app.nodes["behavior.atlas"]["CompositeAtlas"].config.cask = args.cask
app.load("packages/planner/apps/multi_joint_lqr_control.subgraph.json",
prefix="lqr")
# load multi joint lqr control subgraph
lqr_interface = app.nodes["lqr.subgraph"]["interface"]
kinematic_tree = app.nodes["lqr.kinematic_tree"]["KinematicTree"]
lqr_planner = app.nodes["lqr.local_plan"]["MultiJointLqrPlanner"]
app.connect(behavior_interface, "joint_target", lqr_interface,
"joint_target")
kinematic_tree.config.kinematic_file = kinematic_file
lqr_planner.config.speed_min = [-1.5] + [-1.0] * (len(joints) - 1)
lqr_planner.config.speed_max = [1.5] + [1.0] * (len(joints) - 1)
lqr_planner.config.acceleration_min = [-1.5] + [-1.0] * (len(joints) - 1)
