def spin(self):
s = struct.Struct('f f f i B B B B i i i')
s2 = struct.Struct('B B B B B B B B B B B B B B B B B B B B B B B B B B B B B B B B')
rospack = rospkg.RosPack()
env_file=rospack.get_path('velma_scripts') + '/data/jar/cabinet_jar.env.xml'
srdf_path=rospack.get_path('velma_description') + '/robots/'
object_name = 'jar'
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
print "done."
self.listener = tf.TransformListener()
rospy.sleep(0.5)
self.br = tf.TransformBroadcaster()
self.point_cloud_pub = rospy.Publisher("/head_kinect/depth_registered/points", sensor_msgs.msg.PointCloud2, queue_size=100)
self.point_cloud = sensor_msgs.msg.PointCloud2()
self.point_cloud.header.frame_id = 'head_kinect_rgb_optical_frame'
self.point_cloud.header.seq = 0
# self.point_cloud.height = 480
# self.point_cloud.width = 640
self.point_cloud.height = 60
self.point_cloud.width = 80
self.point_cloud.fields.append(sensor_msgs.msg.PointField('x', 0, 7, 1))
self.point_cloud.fields.append(sensor_msgs.msg.PointField('y', 4, 7, 1))
self.point_cloud.fields.append(sensor_msgs.msg.PointField('z', 8, 7, 1))
self.point_cloud.fields.append(sensor_msgs.msg.PointField('rgb', 16, 7, 1))
self.point_cloud.is_bigendian = False
self.point_cloud.point_step = 32
self.point_cloud.row_step = self.point_cloud.point_step * self.point_cloud.width
self.point_cloud.is_dense = False
#
# Initialise Openrave
#
openrave = openraveinstance.OpenraveInstance()
openrave.startOpenrave(viewer=False)
# openrave.runOctomapServer()
openrave.loadEnv(env_file)
# openrave.env.GetCollisionChecker().SetCollisionOptions(0)#4)
collision_models_urdf = {
"velmasimplified0" : ("velma_simplified.srdf", False, True, 0.0, False),
}
openrave.readRobot(srdf_path=srdf_path, collision_models_urdf=collision_models_urdf)
# openrave.addMaskedObjectOctomap("velmasimplified0");
while not rospy.is_shutdown():
time_now, js = velma.getLastJointState()
T_B_C = velma.fk_ik_solver.calculateFk('head_kinect_rgb_optical_frame', js)
openrave.updateRobotConfigurationRos(js)
rospy.sleep(0.1)
time_tf = rospy.Time.now()-rospy.Duration(0.5)
if self.listener.canTransform('world', 'torso_base', time_tf):
pose = self.listener.lookupTransform('world', 'torso_base', time_tf)
else:
print 'cannot transform'
continue
T_W_B = pm.fromTf(pose)
T_W_C = T_W_B * T_B_C
T_C_W = T_W_C.Inverse()
valid_points = 0
self.point_cloud.data = []
for y in range(self.point_cloud.height):
fy = float(y)/float(self.point_cloud.height)-0.5
for x in range(self.point_cloud.width):
fx = float(x)/float(self.point_cloud.width)-0.5
origin = T_W_C * PyKDL.Vector()
d = T_W_C * PyKDL.Vector(fx, fy, 1.0) - origin
d *= 4.0
report = CollisionReport()
ret = openrave.env.CheckCollision(Ray([origin.x(),origin.y(),origin.z()],[d.x(),d.y(),d.z()]), report)
if len(report.contacts) > 0:
x_W, y_W, z_W = report.contacts[0].pos
p_C = T_C_W * PyKDL.Vector(x_W, y_W, z_W)
values = ( p_C.x(), p_C.y(), p_C.z(), 0, 128, 128, 128, 255, 0, 0, 0 )
packed_data = s.pack(*values)
unpacked = s2.unpack(packed_data)
for p in unpacked:
self.point_cloud.data.append(p)
valid_points += 1
else:
for i in range(32):
self.point_cloud.data.append(255)
self.point_cloud.header.seq += 1
self.point_cloud.header.stamp = time_now
self.point_cloud_pub.publish(self.point_cloud)
T_W_J = openrave.getPoseW(object_name)
T_msg = pm.toMsg(T_W_J)
self.br.sendTransform([T_msg.position.x, T_msg.position.y, T_msg.position.z], [T_msg.orientation.x, T_msg.orientation.y, T_msg.orientation.z, T_msg.orientation.w], rospy.Time.now(), object_name, "world")
def spin(self):
self.pub_marker = velmautils.MarkerPublisher()
rospack = rospkg.RosPack()
srdf_path=rospack.get_path('velma_description') + '/robots/'
obj_filenames = [
rospack.get_path('velma_scripts') + '/data/jar/jar.kinbody.xml',
# rospack.get_path('velma_scripts') + '/data/jar/jar_collision.kinbody.xml',
]
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
print "done."
self.listener = tf.TransformListener()
rospy.sleep(0.5)
# self.br = tf.TransformBroadcaster()
#
# Initialise Openrave
#
openrave = openraveinstance.OpenraveInstance()
openrave.startOpenrave(viewer=True)
self.openrave = openrave
openrave.runOctomapServer()
collision_models_urdf = {
"velmasimplified0" : ("velma_simplified.srdf", False, True, 0.0, False),
}
openrave.readRobot(srdf_path=srdf_path, collision_models_urdf=collision_models_urdf)
mo_state = objectstate.MovableObjectsState(openrave.env, obj_filenames)
for filename in obj_filenames:
body = openrave.env.ReadKinBodyXMLFile(filename)
body.Enable(True)
body.SetVisible(True)
openrave.env.Add(body)
openrave.addMaskedObjectToOctomap("velmasimplified0");
self.grasped = {}
s = rospy.Service('change_state', state_server_msgs.srv.ChangeState, self.handle_change_state)
counter = 0
while not rospy.is_shutdown():
time_now, js = velma.getLastJointState()
openrave.updateRobotConfigurationRos(js)
time_tf = rospy.Time.now()-rospy.Duration(0.5)
added, removed = mo_state.update(self.listener, time_tf)
for link_name in self.grasped:
object_name, T_L_O = self.grasped[link_name]
if object_name in mo_state.obj_map:
T_W_L = conv.OpenraveToKDL(openrave.robot_rave.GetLink(link_name).GetTransform())
mo_state.obj_map[object_name].T_W_O = T_W_L * T_L_O
mo_state.updateOpenrave(openrave.env)
for obj_name in added:
openrave.addMaskedObjectToOctomap(obj_name)
for obj_name in removed:
openrave.removeMaskedObjectFromOctomap(obj_name)
# mo_state.updateOpenrave(openrave.env)
if counter % 10 ==0:
m_id = 0
# publish markers
for obj_name in mo_state.obj_map:
obj = mo_state.obj_map[obj_name]
body = openrave.env.GetKinBody(obj_name)
for link in body.GetLinks():
T_W_L = conv.OpenraveToKDL(link.GetTransform())
for geom in link.GetGeometries():
T_L_G = conv.OpenraveToKDL(geom.GetTransform())
g_type = geom.GetType()
if g_type == openravepy_int.GeometryType.Cylinder:
radius = geom.GetCylinderRadius()
height = geom.GetCylinderHeight()
m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(), m_id, r=1, g=0, b=0, a=1, namespace='state_server_objects', frame_id='world', m_type=Marker.CYLINDER, scale=Vector3(radius*2, radius*2, height), T=T_W_L*T_L_G)
self.pub_marker.eraseMarkers(m_id, 1000, frame_id='world', namespace='state_server_objects')
for obj_name in mo_state.obj_map:
openrave.UpdateMaskedObjectInOctomap(obj_name)
rospy.sleep(0.01)
counter += 1
if counter >= 100:
counter = 0
