def test_package_map(self):
dut = PackageMap()
tmpdir = os.environ.get('TEST_TMPDIR')
model = FindResourceOrThrow(
"drake/examples/atlas/urdf/atlas_minimal_contact.urdf")
# Simple coverage test for Add, Contains, size, GetPath.
dut.Add("root", tmpdir)
self.assertEqual(dut.size(), 1)
self.assertTrue(dut.Contains("root"))
self.assertEqual(dut.GetPath("root"), tmpdir)
# Simple coverage test for Drake paths.
dut.PopulateUpstreamToDrake(model)
self.assertGreater(dut.size(), 1)
# Simple coverage test for folder and environment.
dut.PopulateFromEnvironment('TEST_TMPDIR')
dut.PopulateFromFolder(tmpdir)
def test_package_map(self):
dut = PackageMap()
dut2 = PackageMap()
tmpdir = os.environ.get('TEST_TMPDIR')
model = FindResourceOrThrow(
"drake/examples/atlas/urdf/atlas_minimal_contact.urdf")
# Simple coverage test for Add, AddMap, Contains, size,
# GetPackageNames, GetPath, AddPackageXml, Remove.
dut.Add(package_name="root", package_path=tmpdir)
dut2.Add(package_name="root", package_path=tmpdir)
dut.AddMap(dut2)
self.assertTrue(dut.Contains(package_name="root"))
self.assertEqual(dut.size(), 1)
self.assertEqual(dut.GetPackageNames(), ["root"])
self.assertEqual(dut.GetPath(package_name="root"), tmpdir)
dut.AddPackageXml(filename=FindResourceOrThrow(
"drake/multibody/parsing/test/box_package/package.xml"))
dut2.Remove(package_name="root")
self.assertEqual(dut2.size(), 0)
# Simple coverage test for Drake paths.
dut.PopulateUpstreamToDrake(model_file=model)
self.assertGreater(dut.size(), 1)
# Simple coverage test for folder and environment.
dut.PopulateFromEnvironment(environment_variable='TEST_TMPDIR')
dut.PopulateFromFolder(path=tmpdir)
def main():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
package_map = PackageMap()
package_map.PopulateFromEnvironment('AMENT_PREFIX_PATH')
sdf_file_path = os.path.join(package_map.GetPath('iiwa14_description'),
'iiwa14_no_collision.sdf')
joint_names = [
'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4',
'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'
]
joints = []
for name in joint_names:
joints.append(station.get_controller_plant().GetJointByName(name))
rclpy.init()
node = rclpy.create_node('interactive_demo')
# Publish SDF content on robot_description topic
latching_qos = QoSProfile(depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
description_publisher = node.create_publisher(StringMsg,
'robot_description',
qos_profile=latching_qos)
msg = StringMsg()
with open(sdf_file_path, 'r') as sdf_file:
msg.data = sdf_file.read()
description_publisher.publish(msg)
clock_system = SystemClock()
builder.AddSystem(clock_system)
# Transform broadcaster for TF frames
tf_broadcaster = TransformBroadcaster(node)
# Connect to system that publishes TF transforms
tf_system = builder.AddSystem(
TFPublisher(tf_broadcaster=tf_broadcaster, joints=joints))
tf_buffer = Buffer(node=node)
tf_listener = TransformListener(tf_buffer, node)
server = InteractiveMarkerServer(node, 'joint_targets')
joint_target_system = builder.AddSystem(
MovableJoints(server, tf_buffer, joints))
fk_system = builder.AddSystem(
ForwardKinematics(station.get_controller_plant()))
builder.Connect(station.GetOutputPort('iiwa_position_measured'),
fk_system.GetInputPort('joint_positions'))
builder.Connect(fk_system.GetOutputPort('transforms'),
tf_system.GetInputPort('body_poses'))
builder.Connect(clock_system.GetOutputPort('clock'),
tf_system.GetInputPort('clock'))
builder.Connect(joint_target_system.GetOutputPort('joint_states'),
station.GetInputPort('iiwa_position'))
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
constant_sys = builder.AddSystem(ConstantVectorSource(numpy.array([0.107
])))
builder.Connect(constant_sys.get_output_port(0),
station.GetInputPort("wsg_position"))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator_context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(1.0)
station_context = station.GetMyMutableContextFromRoot(simulator_context)
num_iiwa_joints = station.num_iiwa_joints()
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, numpy.zeros(num_iiwa_joints))
while simulator_context.get_time() < 12345:
simulator.AdvanceTo(simulator_context.get_time() + 0.01)
# TODO(sloretz) really need a spin_some in rclpy
rclpy.spin_once(node, timeout_sec=0)
rclpy.shutdown()