<jupyter_start><jupyter_text>Create objects and move the robot toward them<jupyter_code>rospy.init_node('demo')

put_object("cracker_box", 0.6, 1.9, 0.5)
put_object("mustard_bottle", 1.0, 1.8, 0.5)
put_object("mug", 0.8, 1.7, 0.5)
put_object("foam_brick", 1.2, 1.7, 0.5)

# Move toward the table
# look down a little
move_head_tilt(-0.5)

# move position
move_arm_init()
# move in front of the long table
move_base_goal(1, 1, 90)
# Look for objects
time.sleep(1)<jupyter_output><empty_output><jupyter_text>Load Darknet config (YOLOV4)<jupyter_code>darknet_config_dir = os.path.join(os.environ.get('DARKNET_PATH', './'), "darknet_config/")
config_file = darknet_config_dir+"yolov4-tiny-obj.cfg"
data_file = darknet_config_dir+"obj.data"
weights = darknet_config_dir+"yolov4-tiny-obj_last.weights"
random.seed(3)  # deterministic bbox colors
network, class_names, class_colors = darknet.load_network(config_file, data_file, weights, batch_size=1)

prev_time = time.time()

bridge = CvBridge()<jupyter_output><empty_output><jupyter_text>Get the current RGB and Depth Image from ROS<jupyter_code>image_rgb = rospy.wait_for_message('/hsrb/head_rgbd_sensor/rgb/image_rect_color', Image)
image_depth = rospy.wait_for_message('/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw', Image)
pointcloud = rospy.wait_for_message('/hsrb/head_rgbd_sensor/depth_registered/rectified_points', PointCloud2)<jupyter_output><empty_output><jupyter_text>Transform from OpenCV (cv2)<jupyter_code>image_cv2 = bridge.imgmsg_to_cv2(image_rgb, "bgr8")
rospy.get_rostime()
plt.imshow(image_cv2)
plt.show()<jupyter_output><empty_output><jupyter_text>Perform detection using darknet<jupyter_code>def image_detection(image, network, class_names, class_colors, thresh=0.25):
    # Darknet doesn't accept numpy images.
    # Create one with image we reuse for each detect
    # width = darknet.network_width(network)
    # height = darknet.network_height(network)
    width = 640
    height = 480
    height = darknet.network_height(network)
    darknet_image = darknet.make_image(width, height, 3)

    image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    image_resized = cv2.resize(image_rgb, (width, height), interpolation=cv2.INTER_LINEAR)

    darknet.copy_image_from_bytes(darknet_image, image_resized.tobytes())
    detections = darknet.detect_image(network, class_names, darknet_image, thresh=thresh)
    darknet.free_image(darknet_image)
    image = darknet.draw_boxes(detections, image_resized, class_colors)
    return cv2.cvtColor(image, cv2.COLOR_BGR2RGB), detections

image, detections = image_detection(image_cv2, network, class_names, class_colors)
plt.imshow(image)
plt.show()<jupyter_output><empty_output><jupyter_text>Get the box coordinates from the first object<jupyter_code>def convertCoords(x1, y1, w_size, h_size):
    x_start = 	int(round(x1 - (w_size/2)))
    y_start = 	int(round(y1 - (h_size/2)))
    x_end   = 	int(round(x_start + w_size))
    y_end   = 	int(round(y_start + h_size))
    return x_start, y_start, x_end, y_end

x, y, w, h = detections[1][2][0], detections[1][2][1], detections[1][2][2], detections[1][2][3]
x_start, y_start, x_end, y_end = convertCoords(x,y,w,h)<jupyter_output><empty_output><jupyter_text>Get the object corresponding pointcloud by cropping the global pointcloud<jupyter_code>def crop_pointcloud( min_x, min_y, max_x, max_y, width, height, pointcloud_sub):
    points = []
    for i in range(min_x, max_x):
        for j in range(min_y, max_y):
            points.append([i, j])

    points = list(pc2.read_points(pointcloud_sub, field_names=("x", "y", "z"), skip_nans=True, uvs=points))
    return points
    
points = crop_pointcloud(x_start, y_start, x_end, y_end, w, h, pointcloud)

pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

visualizer = JVisualizer()
visualizer.add_geometry(pcd)
visualizer.show()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.12419974391805377, 'fraction_numerical': 0.021254801536491678, 'mean_word_length': 6.233333333333333, 'pattern_counts': {'":': 0, '<': 29, '<?xml version=': 0, '>': 29, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '7', 'dir': 'jupyter-structured-clean-dedup', 'id': '215520', 'license': 'BSD-3-Clause-Clear', 'n_tokens_mistral': 1548, 'n_tokens_neox': 1490, 'n_words': 264, 'path': 'src/robobreizh/scripts/Vision/8_Vision+Grasping.ipynb', 'repo_name': 'Maelic/robocup-at-home-2021-challenge', 'size': '3905'}