def __init__(self, useTwoSensors):
'''Set variables contained in self.'''
CloudProxy.__init__(self)
self.useTwoSensors = useTwoSensors
self.cloud1Msg = None
self.cloud2Msg = None
self.hasCloud1 = True
self.hasCloud2 = True
def __init__(self):
'''Set variables contained in self.'''
CloudProxy.__init__(self)
self.hasCloud = True
self.cloudSub = None
self.cloudMsg = None
self.registerSensors()
self.cloudVisPub = rospy.Publisher("/cloud_rviz",
PointCloud2,
queue_size=1)
def __init__(self):
'''Set variables contained in self.'''
CloudProxy.__init__(self)
self.hasCloud = False
self.cloudSub = None
self.cloudMsg = None
self.registerSensors()
#self.cloudVisPub = rospy.Publisher("/cloud_rviz", PointCloud2, queue_size=1)
self.cloudSub = rospy.Subscriber("/unifier/unified_cloud",
PointCloud2,
self.callback,
queue_size=1)
def __init__(self):
'''Set variables contained in self.'''
CloudProxy.__init__(self)
self.hasVolume = False
self.volumeMsg = None
# infinitam parameters: check whether infinitam has lost track
self.angOffset = 10*(pi/180) # maximum allowed offset between orientations
self.transOffset = 0.05 # maximum allowed offset between positions
# create publishers and subscriber to communicate with infinitam
self.infinitamSub = rospy.Subscriber("/infinitam/volume", VolumeMsg, self.callback)
self.activeCloudPub = rospy.Publisher("/infinitam/publishVolume", Int32Msg, queue_size=1)
# Wait for nodes to come online
print("Waiting for /infinitam/volume to connect ...")
while self.infinitamSub.get_num_connections() == 0:
rospy.sleep(1)
import plot
from cloud_proxy import CloudProxy
import point_cloud_util
from hand_descriptor import HandDescriptor
# point_cloud
import point_cloud
workspace = [(-0.5, 1.0), (0, 1.5), (-0.50, 0.50)]
grasp_offsets = [0.23, 0, 0.16]
grasp_offsets[1] = grasp_offsets[2] + (grasp_offsets[0] -
grasp_offsets[2]) / 2.0
rospy.init_node("edge_grasp")
cloud_proxy = CloudProxy()
grasp_proxy = GraspProxy()
rviz_node = RvizNode()
plot_rviz = PlotRviz()
plot_rviz.initRos()
cloud = point_cloud.LoadPcd(
os.path.dirname(__file__) + '/../data/pcd/cloud5.pcd')
cloud = point_cloud_util.filter_workspace(workspace, cloud)
def create_data():
grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets)
# cloud 2
# grasps = filter(lambda g: (grasp_space[0][0] < g.bottom[0] < grasp_space[0][1] and
# grasp_space[1][0] < g.bottom[1] < grasp_space[1][1] and
import point_cloud
# pytorch
from torch.autograd import Variable
cloud_name = 'cloud2.pcd'
cnn = torch.load(os.path.dirname(__file__) + '/../model/cnn3d')
workspace = [(-0.5, 1.0), (0, 1.5), (-0.50, 0.50)]
grasp_offsets = [0.23, 0, 0.16]
grasp_offsets[1] = grasp_offsets[2] + (grasp_offsets[0] -
grasp_offsets[2]) / 2.0
rospy.init_node("edge_grasp")
cloud_proxy = CloudProxy()
grasp_proxy = GraspProxy()
rviz_node = RvizNode()
plot_rviz = PlotRviz()
plot_rviz.initRos()
cloud = point_cloud.LoadPcd(
os.path.dirname(__file__) + '/../data/pcd/' + cloud_name)
cloud = point_cloud_util.filter_workspace(workspace, cloud)
grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets)
image_3d = []
for grasp in grasps:
hand_des_3d = HandDescriptor(
HandDescriptor.T_from_approach_axis_center(