Running camera tools on your robot

Here is a demo video showing what one can accomplish through this tutorial.

Getting started

  • Make sure robot is turned ON

  • Start the python virtual environment

source ~/pyenv_pyrobot/bin/activate
  • Launch the robot driver
roslaunch locobot_control main.launch use_camera:=true

Example usage

  • Initialize a robot instance
from pyrobot import Robot

bot = Robot('locobot')

bot.camera.reset()
  • Set camera pan or tilt angle, or set pan and tilt angles together
pan = 0.4
bot.camera.set_pan(pan, wait=True)
tilt = 0.4
bot.camera.set_tilt(tilt, wait=True)
camera_pose = [0, 0.7]
bot.camera.set_pan_tilt(camera_pose[0], camera_pose[1], wait=True)
  • Get rgb or depth images
import cv2
rgb = bot.camera.get_rgb()
cv2.imshow('Color', rgb[:, :, ::-1])
cv2.waitKey(3000)
import cv2
import numpy as np
depth = bot.camera.get_depth()
actual_depth_values = depth.astype(np.float64) / 1000.0
cv2.imshow('Depth', depth)
cv2.waitKey(3000)
import cv2
rgb, depth = bot.camera.get_rgb_depth()
cv2.imshow('Color', rgb[:, :, ::-1])
cv2.imshow('Depth', depth)
cv2.waitKey(3000)
  • Get 3D point coordinates of pixels in RGB image

In manipulation, we often get into the case where we want to know where an RGB pixel is in the 3D world, meaning the correspondence between RGB pixels and their 3D locations with respect to the robot. In PyRobot, we have provided a utility function (pix_to_3dpt) to do this transformation. This function takes as input the row indices and column indices of the pixels. It will return the 3D point locations and the RGB color values of these pixels. You can specify in_cam=True to get the 3D locations in the camera frame. Otherwise, the 3D locations will be with respect to the LoCoBot base frame.

bot.camera.set_tilt(0.6)
# row number in the RGB image
r = 295
# column number in the RGB image
c = 307
pt, color = bot.camera.pix_to_3dpt(r,c)
print('3D point:', pt)
print('Color:', color)
rgb = bot.camera.get_rgb()
cv2.imshow('Color', rgb[:, :, ::-1])
cv2.waitKey(10000)
bot.camera.set_tilt(0.6)
r = [295, 360]
c = [307, 296]
# this will return the point and color of 
# pixel (295, 307) and pixel (360, 296)
pt, color = bot.camera.pix_to_3dpt(r,c)
print('3D point:', pt)
print('Color:', color)
rgb = bot.camera.get_rgb()
cv2.imshow('Color', rgb[:, :, ::-1])
cv2.waitKey(10000)
import numpy as np
bot.camera.set_tilt(0.6)
r = np.array([295, 360])
c = np.array([307, 296])
# this will return the point and color of 
# pixel (295, 307) and pixel (360, 296)
pt, color = bot.camera.pix_to_3dpt(r,c)
print('3D point:', pt)
print('Color:', color)
rgb = bot.camera.get_rgb()
cv2.imshow('Color', rgb[:, :, ::-1])
cv2.waitKey(10000)
← [Basic] Pushing[Advanced] Grasping →