# 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"))