# Or can swap out the "new" and instead connect to an existing meshcat-server instance
# meshcat_vis = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url="new", open_browser=False)
plant.Finalize()
diagram = builder.Build()
# Creating top level context and extracting subsystem contexts
diagram_context = diagram.CreateDefaultContext()
sensor_context = sensor.GetMyMutableContextFromRoot(diagram_context)
sg_context = scene_graph.GetMyMutableContextFromRoot(diagram_context)
simulator = Simulator(diagram)
simulator.Initialize()
#### Plotting stuff
color = sensor.color_image_output_port().Eval(sensor_context).data
label = sensor.label_image_output_port().Eval(sensor_context).data
fig, ax = plt.subplots(1, 2, figsize=(10, 5))
ax[0].imshow(color)
ax[1].imshow(colorize_labels(label))
query_object = scene_graph.get_query_output_port().Eval(sg_context)
inspector = query_object.inspector()
label_by_model = label.copy()
for geometry_id in inspector.GetAllGeometryIds():
body = plant.GetBodyFromFrameId(inspector.GetFrameId(geometry_id))
geometry_label = inspector.GetPerceptionProperties(
geometry_id).GetProperty("label", "id")
# WARNING: If you do not cast the `geometry_label` to `int`, this
# comparison will take a long time since NumPy will do
camera_instance.query_object_input_port())
#add to the dictionary of cameras, with no image associated for now
camera_images_rgb[camera.name] = np.zeros([480, 640, 4])
camera_images_depth[camera.name] = np.zeros([480, 640, 1])
color_info = camera_instance.color_camera_info()
depth_info = camera_instance.depth_camera_info()
cloud_generator_instance = DepthImageToPointCloud(depth_info)
builder.AddSystem(cloud_generator_instance)
builder.Connect(camera_instance.depth_image_32F_output_port(),
cloud_generator_instance.depth_image_input_port())
image_to_lcm_image_array = builder.AddSystem(ImageToLcmImageArrayT())
image_to_lcm_image_array.set_name("converter")
cam_port = (image_to_lcm_image_array.DeclareImageInputPort[
PixelType.kRgba8U]("camera_" + str(0)))
builder.Connect(camera_instance.color_image_output_port(), cam_port)
image_array_lcm_publisher = builder.AddSystem(
LcmPublisherSystem.Make(channel="DRAKE_RGBD_CAMERA_IMAGES",
lcm_type=image_array_t,
lcm=None,
publish_period=0.03,
use_cpp_serializer=True))
image_array_lcm_publisher.set_name("rgbd_publisher")
builder.Connect(image_to_lcm_image_array.image_array_t_msg_output_port(),
image_array_lcm_publisher.get_input_port(0))
# viz = builder.AddSystem(MeshcatVisualizer(scene_graph))
# builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0))
# pc_viz = builder.AddSystem(MeshcatPointCloudVisualizer(viz, viz.draw_period))
# builder.Connect(cloud_generator_instance['camera_hand'].point_cloud_output_port(), pc_viz.GetInputPort("point_cloud_P"))