def main(args):
# set logging
logging.getLogger().setLevel(logging.INFO)
rospy.init_node("ensenso_reader", anonymous=True)
num_frames = 10
sensor = RgbdSensorFactory("ensenso", cfg={"frame": "ensenso"})
sensor.start()
total_time = 0
for i in range(num_frames):
if i > 0:
start_time = time.time()
_, depth_im, _ = sensor.frames()
if i > 0:
total_time += time.time() - start_time
print("Frame %d" % (i))
print("Avg FPS: %.5f" % (float(i) / total_time))
depth_im = sensor.median_depth_img(num_img=5)
point_cloud = sensor.ir_intrinsics.deproject(depth_im)
point_cloud.remove_zero_points()
sensor.stop()
vis2d.figure()
vis2d.imshow(depth_im)
vis2d.title("Ensenso - Raw")
vis2d.show()
vis3d.figure()
vis3d.points(point_cloud, random=True, subsample=10, scale=0.0025)
vis3d.show()
def test_GQCNN():
config = YamlConfig(
'/home/baymax/catkin_ws/src/jaco_manipulation/cfg/grasp_test.yaml')
# create rgbd sensor
rospy.loginfo('Creating RGBD Sensor')
sensor_cfg = config['sensor_cfg']
sensor_type = sensor_cfg['type']
sensor = RgbdSensorFactory.sensor(sensor_type, sensor_cfg)
sensor.start()
rospy.loginfo('Sensor Running')
# setup safe termination
def handler(signum, frame):
rospy.loginfo('caught CTRL+C, exiting...')
if sensor is not None:
sensor.stop()
if robot is not None:
robot.stop()
if subscriber is not None and subscriber._started:
subscriber.stop()
exit(0)
signal.signal(signal.SIGINT, handler)
planner = GQCNNPlanner(sensor, config)
# rospy.sleep(10)
planner.get_grasp_plan("cup")
def main(args):
# from visualization import Visualizer2D as vis2d
# from visualization import Visualizer3D as vis3d
import matplotlib.pyplot as vis2d
# set logging
logging.getLogger().setLevel(logging.DEBUG)
rospy.init_node("kinect_reader", anonymous=True)
num_frames = 5
sensor_cfg = {
"quality": Kinect2BridgedQuality.HD,
"frame": "kinect2_rgb_optical_frame",
}
sensor_type = "bridged_kinect2"
sensor = RgbdSensorFactory.sensor(sensor_type, sensor_cfg)
sensor.start()
def handler(signum, frame):
rospy.loginfo("caught CTRL+C, exiting...")
if sensor is not None:
sensor.stop()
exit(0)
signal.signal(signal.SIGINT, handler)
total_time = 0
for i in range(num_frames):
if i > 0:
start_time = time.time()
_, depth_im, _ = sensor.frames()
if i > 0:
total_time += time.time() - start_time
logging.info("Frame %d" % (i))
logging.info("Avg FPS: %.5f" % (float(i) / total_time))
depth_im = sensor.median_depth_img(num_img=5)
color_im, depth_im, _ = sensor.frames()
sensor.stop()
vis2d.figure()
vis2d.subplot("211")
vis2d.imshow(depth_im.data)
vis2d.title("Kinect - depth Raw")
vis2d.subplot("212")
vis2d.imshow(color_im.data)
vis2d.title("kinect color")
vis2d.show()
def main():
logging.getLogger().setLevel(logging.INFO)
# parse args
parser = argparse.ArgumentParser(description='Register a webcam to the Photoneo PhoXi')
parser.add_argument('--config_filename', type=str, default='cfg/tools/colorize_phoxi.yaml', help='filename of a YAML configuration for registration')
args = parser.parse_args()
config_filename = args.config_filename
config = YamlConfig(config_filename)
sensor_data = config['sensors']
phoxi_config = sensor_data['phoxi']
phoxi_config['frame'] = 'phoxi'
# Initialize ROS node
rospy.init_node('colorize_phoxi', anonymous=True)
logging.getLogger().addHandler(rl.RosStreamHandler())
# Get PhoXi sensor set up
phoxi = RgbdSensorFactory.sensor(phoxi_config['type'], phoxi_config)
phoxi.start()
# Capture PhoXi and webcam images
phoxi_color_im, phoxi_depth_im, _ = phoxi.frames()
# vis2d.figure()
# vis2d.subplot(121)
# vis2d.imshow(phoxi_color_im)
# vis2d.subplot(122)
# vis2d.imshow(phoxi_depth_im)
# vis2d.show()
phoxi_pc = phoxi.ir_intrinsics.deproject(phoxi_depth_im)
colors = phoxi_color_im.data.reshape((phoxi_color_im.shape[0] * phoxi_color_im.shape[1], phoxi_color_im.shape[2])) / 255.0
vis3d.figure()
vis3d.points(phoxi_pc.data.T[::3], color=colors[::3], scale=0.001)
vis3d.show()
# Export to PLY file
vertices = phoxi.ir_intrinsics.deproject(phoxi_depth_im).data.T
colors = phoxi_color_im.data.reshape(phoxi_color_im.data.shape[0] * phoxi_color_im.data.shape[1], phoxi_color_im.data.shape[2])
f = open('pcloud.ply', 'w')
f.write('ply\nformat ascii 1.0\nelement vertex {}\nproperty float x\nproperty float y\nproperty float z\nproperty uchar red\n'.format(len(vertices)) +
'property uchar green\nproperty uchar blue\nend_header\n')
for v, c in zip(vertices,colors):
f.write('{} {} {} {} {} {}\n'.format(v[0], v[1], v[2], c[0], c[1], c[2]))
f.close()
def main():
ids = discover_cams()
assert ids, "[!] No camera detected."
cfg = {}
cfg["cam_id"] = ids[0]
cfg["filter_depth"] = True
cfg["frame"] = "realsense_overhead"
sensor = RgbdSensorFactory.sensor("realsense", cfg)
sensor.start()
camera_intr = sensor.color_intrinsics
color_im, depth_im, _ = sensor.frames()
sensor.stop()
print("intrinsics matrix: {}".format(camera_intr.K))
_, axes = plt.subplots(1, 2)
for ax, im in zip(axes, [color_im.data, depth_im.data]):
ax.imshow(im)
ax.axis("off")
plt.show()
T_cb_world = RigidTransform.load(config['chessboard_tf'])
# initialize node
rospy.init_node('register_camera', anonymous=True)
# get camera sensor object
for sensor_frame, sensor_data in config['sensors'].iteritems():
sensor_config = sensor_data['sensor_config']
registration_config = sensor_data['registration_config'].copy()
registration_config.update(config['chessboard_registration'])
# open sensor
try:
sensor_type = sensor_config['type']
sensor_config['frame'] = sensor_frame
sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
logging.info('Starting sensor')
sensor.start()
ir_intrinsics = sensor.ir_intrinsics
logging.info('Sensor initialized')
# register
reg_result = CameraChessboardRegistration.register(sensor, registration_config)
T_camera_world = T_cb_world * reg_result.T_camera_cb
logging.info('Final Result for sensor %s' %(sensor_frame))
logging.info('Rotation: ')
logging.info(T_camera_world.rotation)
logging.info('Quaternion: ')
logging.info(T_camera_world.quaternion)
logging.info('Translation: ')
if not os.path.exists(output_dir):
os.mkdir(output_dir)
# read config
config = YamlConfig(config_filename)
sensor_type = config['sensor']['type']
sensor_frame = config['sensor']['frame']
# read camera calib
tf_filename = '%s_to_world.tf' % (sensor_frame)
T_camera_world = RigidTransform.load(
os.path.join(config['calib_dir'], sensor_frame, tf_filename))
T_camera_world.save(os.path.join(output_dir, tf_filename))
# setup sensor
sensor = RgbdSensorFactory.sensor(sensor_type, config['sensor'])
# start the sensor
sensor.start()
color_intrinsics = sensor.color_intrinsics
ir_intrinsics = sensor.ir_intrinsics
color_intrinsics.save(
os.path.join(output_dir, '%s_color.intr' % (sensor.frame)))
ir_intrinsics.save(os.path.join(output_dir, '%s_ir.intr' % (sensor.frame)))
# get raw images
for i in range(config['num_images']):
logging.info('Capturing image %d' % (i))
color, depth, ir = sensor.frames()
color.save(os.path.join(output_dir, 'color_%d.png' % (i)))
depth.save(os.path.join(output_dir, 'depth_%d.npy' % (i)))
def register_ensenso(config):
# set parameters
average = True
add_to_buffer = True
clear_buffer = False
decode = True
grid_spacing = -1
# read parameters
num_patterns = config["num_patterns"]
max_tries = config["max_tries"]
# load tf pattern to world
T_pattern_world = RigidTransform.load(config["pattern_tf"])
# initialize sensor
sensor_frame = config["sensor_frame"]
sensor_config = {"frame": sensor_frame}
logging.info("Creating sensor")
sensor = RgbdSensorFactory.sensor("ensenso", sensor_config)
# initialize node
rospy.init_node("register_ensenso", anonymous=True)
rospy.wait_for_service("/%s/collect_pattern" % (sensor_frame))
rospy.wait_for_service("/%s/estimate_pattern_pose" % (sensor_frame))
# start sensor
print("Starting sensor")
sensor.start()
time.sleep(1)
print("Sensor initialized")
# perform registration
try:
print("Collecting patterns")
num_detected = 0
i = 0
while num_detected < num_patterns and i < max_tries:
collect_pattern = rospy.ServiceProxy(
"/%s/collect_pattern" % (sensor_frame), CollectPattern)
resp = collect_pattern(add_to_buffer, clear_buffer, decode,
grid_spacing)
if resp.success:
print("Detected pattern %d" % (num_detected))
num_detected += 1
i += 1
if i == max_tries:
raise ValueError("Failed to detect calibration pattern!")
print("Estimating pattern pose")
estimate_pattern_pose = rospy.ServiceProxy(
"/%s/estimate_pattern_pose" % (sensor_frame), EstimatePatternPose)
resp = estimate_pattern_pose(average)
q_pattern_camera = np.array([
resp.pose.orientation.w,
resp.pose.orientation.x,
resp.pose.orientation.y,
resp.pose.orientation.z,
])
t_pattern_camera = np.array(
[resp.pose.position.x, resp.pose.position.y, resp.pose.position.z])
T_pattern_camera = RigidTransform(
rotation=q_pattern_camera,
translation=t_pattern_camera,
from_frame="pattern",
to_frame=sensor_frame,
)
except rospy.ServiceException as e:
print("Service call failed: %s" % (str(e)))
# compute final transformation
T_camera_world = T_pattern_world * T_pattern_camera.inverse()
# save final transformation
output_dir = os.path.join(config["calib_dir"], sensor_frame)
if not os.path.exists(output_dir):
os.makedirs(output_dir)
pose_filename = os.path.join(output_dir, "%s_to_world.tf" % (sensor_frame))
T_camera_world.save(pose_filename)
intr_filename = os.path.join(output_dir, "%s.intr" % (sensor_frame))
sensor.ir_intrinsics.save(intr_filename)
# log final transformation
print("Final Result for sensor %s" % (sensor_frame))
print("Rotation: ")
print(T_camera_world.rotation)
print("Quaternion: ")
print(T_camera_world.quaternion)
print("Translation: ")
print(T_camera_world.translation)
# stop sensor
sensor.stop()
# move robot to calib pattern center
if config["use_robot"]:
# create robot pose relative to target point
target_pt_camera = Point(T_pattern_camera.translation, sensor_frame)
target_pt_world = T_camera_world * target_pt_camera
R_gripper_world = np.array([[1.0, 0, 0], [0, -1.0, 0], [0, 0, -1.0]])
t_gripper_world = np.array([
target_pt_world.x + config["gripper_offset_x"],
target_pt_world.y + config["gripper_offset_y"],
target_pt_world.z + config["gripper_offset_z"],
])
T_gripper_world = RigidTransform(
rotation=R_gripper_world,
translation=t_gripper_world,
from_frame="gripper",
to_frame="world",
)
# move robot to pose
y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
y.reset_home()
time.sleep(1)
T_lift = RigidTransform(translation=(0, 0, 0.05),
from_frame="world",
to_frame="world")
T_gripper_world_lift = T_lift * T_gripper_world
y.right.goto_pose(T_gripper_world_lift)
y.right.goto_pose(T_gripper_world)
# wait for human measurement
keyboard_input("Take measurement. Hit [ENTER] when done")
y.right.goto_pose(T_gripper_world_lift)
y.reset_home()
y.stop()
def __init__(self):
super(PickAndDropProgram, self).__init__()
# First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_and_drop_program', anonymous=True)
filepath = rospy.get_param('~goal_joint_states')
goal_joint_states = joint_state_filereader.read(filepath)
self.goal_names = ["home", "near_box", "near_drop", "drop"]
for name in self.goal_names:
assert name in goal_joint_states, \
"Joint state name '{}' is not found".format(name)
# Declare suctionpad topics
self.pub_to = rospy.Publisher('toArduino', String, queue_size=100)
self.pub_from = rospy.Publisher('fromArduino', String, queue_size=100)
# Vision config
config = YamlConfig(rospy.get_param('~config'))
# Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
# the robot:
robot = moveit_commander.RobotCommander()
# Instantiate a `PlanningSceneInterface`_ object. This object is an interface
# to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
# Instantiate a `MoveGroupCommander`_ object. This object is an interface
# to one group of joints. In this case the group is the joints in the UR5
# arm so we set ``group_name = manipulator``. If you are using a different robot,
# you should change this value to the name of your robot arm planning group.
group_name = "manipulator" # See .srdf file to get available group names
group = moveit_commander.MoveGroupCommander(group_name)
# We create a `DisplayTrajectory`_ publisher which is used later to publish
# trajectories for RViz to visualize:
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory,
# queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print("============ Reference frame: %s" % planning_frame)
# We can also print(the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print("============ End effector: %s" % eef_link)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print("============ Robot Groups:", robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(robot.get_current_state())
print("")
# Setup vision sensor
print("============ Setup vision sensor")
# create rgbd sensor
rospy.loginfo('Creating RGBD Sensor')
sensor_cfg = config['sensor_cfg']
sensor_type = sensor_cfg['type']
self.sensor = RgbdSensorFactory.sensor(sensor_type, sensor_cfg)
self.sensor.start()
rospy.loginfo('Sensor Running')
rospy.loginfo('Loading T_camera_world')
tf_camera_world = RigidTransform.load(
rospy.get_param('~world_camera_tf'))
rospy.loginfo('tf_camera_world: {}'.format(tf_camera_world))
# Setup client for grasp pose service
rospy.loginfo('Setup client for grasp pose service')
rospy.wait_for_service('grasping_policy')
self.plan_grasp_client = rospy.ServiceProxy('grasping_policy',
GQCNNGraspPlanner)
self.transform_broadcaster = tf2_ros.TransformBroadcaster()
# Misc variables
self.robot = robot
self.scene = scene
self.group = group
self.group_names = group_names
self.config = config
self.tf_camera_world = tf_camera_world
self.goal_joint_states = goal_joint_states
config = YamlConfig('/home/autolab/Workspace/vishal_working/catkin_ws/src/gqcnn/cfg/ros_nodes/yumi_control_node.yaml')
rospy.loginfo('Loading Gripper')
gripper = RobotGripper.load('yumi_metal_spline')
rospy.loginfo('Loading T_camera_world')
T_camera_world = RigidTransform.load('/home/autolab/Public/alan/calib/primesense_overhead/primesense_overhead_to_world.tf')
detector_cfg = config['detector']
# create rgbd sensor
rospy.loginfo('Creating RGBD Sensor')
sensor_cfg = config['sensor_cfg']
sensor_type = sensor_cfg['type']
sensor = RgbdSensorFactory.sensor(sensor_type, sensor_cfg)
sensor.start()
rospy.loginfo('Sensor Running')
# setup safe termination
def handler(signum, frame):
logging.info('caught CTRL+C, exiting...')
if sensor is not None:
sensor.stop()
if robot is not None:
robot.stop()
if subscriber is not None and subscriber._started:
subscriber.stop()
exit(0)
signal.signal(signal.SIGINT, handler)
config = YamlConfig(config_filename)
sensor_type = config["sensor"]["type"]
sensor_frame = config["sensor"]["frame"]
num_grasp_samples = config["num_grasp_samples"]
gripper_width = config["gripper_width"]
inpaint_rescale_factor = config["inpaint_rescale_factor"]
visualize_sampling = config["visualize_sampling"]
sample_config = config["sampling"]
# Read camera calib.
tf_filename = "%s_to_world.tf" % (sensor_frame)
T_camera_world = RigidTransform.load(
os.path.join(config["calib_dir"], sensor_frame, tf_filename))
# Setup sensor.
sensor = RgbdSensorFactory.sensor(sensor_type, config["sensor"])
sensor.start()
camera_intr = sensor.ir_intrinsics
# Read images.
color_im, depth_im, _ = sensor.frames()
color_im = color_im.inpaint(rescale_factor=inpaint_rescale_factor)
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
# Sample grasps.
grasp_sampler = AntipodalDepthImageGraspSampler(sample_config,
gripper_width)
grasps = grasp_sampler.sample(rgbd_im,
camera_intr,
num_grasp_samples,
def test_virtual(self, height=100, width=100):
# Generate folder of color and depth images
if not os.path.exists(IM_FILEROOT):
os.makedirs(IM_FILEROOT)
cam_intr = CameraIntrinsics(
"a",
fx=0.0,
fy=0.0,
cx=0.0,
cy=0.0,
skew=0.0,
height=100,
width=100,
)
cam_intr.save(os.path.join(IM_FILEROOT, "a.intr"))
color_data = (255 * np.random.rand(10, height, width, 3)).astype(
np.uint8)
depth_data = np.random.rand(10, height, width).astype(np.float32)
for i in range(10):
im = ColorImage(color_data[i], frame="a")
im.save(os.path.join(IM_FILEROOT, "color_{:d}.png".format(i)))
im = DepthImage(depth_data[i], frame="a")
im.save(os.path.join(IM_FILEROOT, "depth_{:d}.npy".format(i)))
# Create virtual camera
virtual_cam = RgbdSensorFactory.sensor("virtual",
cfg={
"image_dir": IM_FILEROOT,
"frame": "a"
})
self.assertTrue(
virtual_cam.path_to_images == IM_FILEROOT,
msg="img path changed after init",
)
# Start virtual camera and read frames
virtual_cam.start()
self.assertTrue(virtual_cam.is_running,
msg="camera not running after start")
for i in range(10):
color, depth = virtual_cam.frames()
self.assertTrue(
np.all(color.data == color_data[i]),
msg="color data for img {:d} changed".format(i),
)
self.assertTrue(
color.frame == virtual_cam.frame,
msg="frame mismatch between color im and camera",
)
self.assertTrue(
np.all(depth.data == depth_data[i]),
msg="depth data for img {:d} changed".format(i),
)
self.assertTrue(
depth.frame == virtual_cam.frame,
msg="frame mismatch between depth im and camera",
)
# Make sure camera is stopped
virtual_cam.stop()
self.assertFalse(virtual_cam.is_running,
msg="camera running after stop")
# Cleanup images
for i in range(10):
os.remove(os.path.join(IM_FILEROOT, "color_{:d}.png".format(i)))
os.remove(os.path.join(IM_FILEROOT, "depth_{:d}.npy".format(i)))
os.remove(os.path.join(IM_FILEROOT, "a.intr"))
os.rmdir(IM_FILEROOT)
work_bin = yaml_obj_loader('bin')
work_bin.pose = bin_tf
phoxi = yaml_obj_loader('phoxi')
phoxi.pose = phoxi_tf
# Create scene for each camera for rendering sim
depth_scene = create_scene(phoxi, [work_bin])
# Create ICP objects
feature_matcher = PointToPlaneFeatureMatcher()
solver = PointToPlaneICPSolver()
# Start physical depth and color cameras
logging.info('Creating Phoxi sensor')
sensor_config = {'frame': 'phoxi', 'device_name': 1703005, 'size': 'small'}
phoxi_sensor = RgbdSensorFactory.sensor('phoxi', sensor_config)
logging.info('Starting Phoxi sensor')
phoxi_sensor.start()
logging.info('Phoxi sensor initialized')
# Create box for filtering point cloud
min_pt = np.array([0.25, -0.12, 0])
max_pt = np.array([0.5, 0.12, 0.25])
mask_box = Box(min_pt, max_pt, 'world')
env = GraspingEnv(config, config['vis'])
dataset = get_dataset(config, args)
datapoint = dataset.datapoint_template
obj_id, obj_id_to_key = 0, {}
while not rospy.is_shutdown():
