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