def setMeshPath(self, meshPath, scale):
self.meshPath = meshPath
self.scale = scale
# Set up 3D actor for organ
meshPath = cleanResourcePath(self.meshPath)
extension = os.path.splitext(meshPath)[1]
if extension == ".stl" or extension == ".STL":
meshReader = vtk.vtkSTLReader()
elif extension == ".obj" or extension == ".OBJ":
meshReader = vtk.vtkOBJReader()
else:
ROS_FATAL("Mesh file has invalid extension (" + extension + ")")
meshReader.SetFileName(meshPath)
# Scale STL
transform = vtk.vtkTransform()
transform.Scale(scale, scale, scale)
transformFilter = vtk.vtkTransformFilter()
transformFilter.SetTransform(transform)
transformFilter.SetInputConnection(meshReader.GetOutputPort())
transformFilter.Update()
color = (0,0,1)
self._updateActorPolydata(self.actor_moving,
polydata=transformFilter.GetOutput(),
color = color)
if image is not None:
# Set texture to default
image = cv2.imread(cleanResourcePath(self.texturePath))
self.actor_moving.setTexture(image)
self.actor_moving.textureOnOff(True)
def renderSetup(self):
if type(self.masterWidget) != type(None):
self.actor_moving = self.masterWidget.actor_moving
else:
# Set up 3D actor for organ
meshPath = cleanResourcePath(self.meshPath)
extension = os.path.splitext(meshPath)[1]
if extension == ".stl" or extension == ".STL":
meshReader = vtk.vtkSTLReader()
elif extension == ".obj" or extension == ".OBJ":
meshReader = vtk.vtkOBJReader()
else:
ROS_FATAL("Mesh file has invalid extension (" + extension +
")")
meshReader.SetFileName(meshPath)
# Scale STL
transform = vtk.vtkTransform()
transform.Scale(self.scale, self.scale, self.scale)
transformFilter = vtk.vtkTransformFilter()
transformFilter.SetTransform(transform)
transformFilter.SetInputConnection(meshReader.GetOutputPort())
transformFilter.Update()
color = (0, 0, 1)
self.actor_moving = vtkRosTextureActor("stiffness_texture",
color=color)
self.actor_moving.GetProperty().BackfaceCullingOn()
self._updateActorPolydata(self.actor_moving,
polydata=transformFilter.GetOutput(),
color=color)
# Set texture to default
image = cv2.imread(cleanResourcePath(self.texturePath))
self.actor_moving.setTexture(image)
self.actor_moving.textureOnOff(True)
# Hide actor
self.actor_moving.VisibilityOff()
# Add actor
self.vtkWidget.ren.AddActor(self.actor_moving)
# Setup interactor
self.iren = self.vtkWidget.GetRenderWindow().GetInteractor()
self.iren.RemoveObservers('LeftButtonPressEvent')
self.iren.RemoveObservers('LeftButtonReleaseEvent')
self.iren.RemoveObservers('MouseMoveEvent')
self.iren.RemoveObservers('MiddleButtonPressEvent')
self.iren.RemoveObservers('MiddleButtonPressEvent')
# Set up subscriber for registered organ position
poseSubTopic = "registration_marker"
self.poseSub = rospy.Subscriber(poseSubTopic, Marker,
self.poseCallback)
# Set up QT slider for opacity
self.opacitySlider.valueChanged.connect(self.sliderChanged)
self.textureCheckBox.stateChanged.connect(self.checkBoxChanged)
def __init__(self,
stiffnessTopic,
texturePath,
masterWidget=None,
parent=None):
super(ROIWidget, self).__init__(parent)
# Load UI
uiPath = cleanResourcePath(
"package://oct_15_demo/scripts/roi_widget.ui")
uic.loadUi(uiPath, self)
# Load texture
image = cv2.imread(cleanResourcePath(texturePath))
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
self.originalImage = image
if image.shape[0] > 512 or image.shape[1] > 512:
height, width = min(image.shape[0], 512), min(image.shape[1], 512)
image = cv2.resize(image, (width, height))
self.cvWidget = OverlayInteractorWidget(image,
stiffnessTopic,
parent=self)
self.m = masterWidget
if type(self.m) != type(None):
self.cvWidget.mouseMoveEvent = self.m.cvWidget.mouseMoveEvent
self.cvWidget.mousePressEvent = self.m.cvWidget.mousePressEvent
self.cvWidget.mouseReleaseEvent = self.m.cvWidget.mouseReleaseEvent
self.cvWidget.timerEvent = self.childTimerEvent
self.getStiffness.clicked.connect(self.m.cvWidget.overlayStiffness)
self.clearStiffness.clicked.connect(self.m.cvWidget.clearStiffness)
self.stop.clicked.connect(self.m.cvWidget.stopOverlayStiffness)
else:
rospy.Subscriber(stiffnessTopic,
Image,
self.stiffness_map_callback,
queue_size=1)
self.imgPub = rospy.Publisher('stiffness_texture',
Image,
queue_size=10)
self.getStiffness.clicked.connect(self.cvWidget.overlayStiffness)
self.clearStiffness.clicked.connect(self.cvWidget.clearStiffness)
self.stop.clicked.connect(self.cvWidget.stopOverlayStiffness)
mainLayout = QtWidgets.QHBoxLayout()
mainLayout.addWidget(self.cvWidget)
# mainLayout.addWidget(self.button)
self.cvWindow.setLayout(mainLayout)
self.resize(600, 600)
def __init__(self,
camera,
texturePath,
meshPath,
scale=1,
masterWidget=None,
parent=None):
super(OverlayWidget, self).__init__()
uiPath = cleanResourcePath(
"package://dvrk_vision/src/dvrk_vision/overlay_widget.ui")
# Get CV image from path
uic.loadUi(uiPath, self)
self.meshPath = meshPath
self.scale = scale
self.texturePath = texturePath
self.vtkWidget = QVTKStereoViewer(camera, parent=self)
self.vtkWidget.renderSetup = self.renderSetup
# Add vtk widget
self.vl = QVBoxLayout()
self.vl.addWidget(self.vtkWidget)
self.vtkFrame.setLayout(self.vl)
self.masterWidget = masterWidget
self.otherWindows = []
if type(self.masterWidget) != type(None):
self.masterWidget.otherWindows.append(self)
self.otherWindows.append(self.masterWidget)
self.vtkWidget.Initialize()
self.vtkWidget.start()
def loadMesh(path, scale):
# Read in STL
meshPath = cleanResourcePath(path)
extension = os.path.splitext(path)[1]
if extension == ".stl" or extension == ".STL":
meshInput = vtk.vtkSTLReader()
meshInput.SetFileName(path)
meshReader = vtk.vtkTextureMapToPlane()
meshReader.SetInputConnection(meshInput.GetOutputPort())
elif extension == ".obj" or extension == ".OBJ":
meshReader = vtk.vtkOBJReader()
meshReader.SetFileName(path)
else:
ROS_FATAL("Mesh file has invalid extension (" + extension + ")")
# Scale STL
transform = vtk.vtkTransform()
transform.Scale(scale, scale, scale)
# transform.RotateWXYZ(rot[1], rot[2], rot[3], rot[0])
# transform.Translate(pos[0],pos[1], pos[2])
transformFilter = vtk.vtkTransformFilter()
transformFilter.SetTransform(transform)
transformFilter.SetInputConnection(meshReader.GetOutputPort())
transformFilter.Update()
return transformFilter.GetOutput()
def __init__(self,
camera,
tfBuffer,
markerTopic,
robotFrame,
tipFrame,
cameraFrame,
masterWidget=None,
parent=None):
super(GpOverlayWidget, self).__init__(parent=parent)
# Load in variables
self.tfBuffer = tfBuffer
self.robotFrame = robotFrame
self.tipFrame = tipFrame
self.markerTopic = markerTopic
self.cameraFrame = cameraFrame
self.masterWidget = masterWidget
uiPath = cleanResourcePath(
"package://dvrk_vision/src/dvrk_vision/overlay_widget.ui")
# Get CV image from path
uic.loadUi(uiPath, self)
self.vtkWidget = QVTKStereoViewer(camera, parent=self)
self.vtkWidget.renderSetup = self.renderSetup
# Add vtk widget
self.vl = QVBoxLayout()
self.vl.addWidget(self.vtkWidget)
self.vtkFrame.setLayout(self.vl)
self.otherWindows = []
if self.masterWidget is not None:
self.masterWidget.otherWindows.append(self)
self.otherWindows.append(self.masterWidget)
# Set up QT slider for opacity
self.opacitySlider.valueChanged.connect(self.sliderChanged)
self.textureCheckBox.stateChanged.connect(self.checkBoxChanged)
self.iren = self.vtkWidget.GetRenderWindow().GetInteractor()
self.iren.SetInteractorStyle(vtk.vtkInteractorStyleTrackballActor())
self.organFrame = None
self.textureCheckBox.setText("Show GP mesh")
self.POICheckBox = QCheckBox("Show markers", self)
self.POICheckBox.setChecked(True)
self.POICheckBox.stateChanged.connect(self.checkBoxChanged)
self.horizontalLayout.addWidget(self.POICheckBox)
self.clearButton = QPushButton("Clear markers", self)
self.clearButton.pressed.connect(self.clearPOI)
self.horizontalLayout.addWidget(self.clearButton)
self.vtkWidget.Initialize()
self.vtkWidget.start()
def markerCB(self, data):
meshPath = cleanResourcePath(data.mesh_resource)
if meshPath != self.meshPath:
self.meshPath = meshPath
try:
self.organFrame = data.header.frame_id
if self.organFrame[0] == "/":
self.organFrame = self.organFrame[1:]
poseCamera = self.tfBuffer.lookup_transform(
self.cameraFrame, self.organFrame, rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logwarn(e)
return
posCam = poseCamera.transform.translation
rotCam = poseCamera.transform.rotation
matCam = transformations.quaternion_matrix(
[rotCam.x, rotCam.y, rotCam.z, rotCam.w])
matCam[0:3, 3] = [posCam.x, posCam.y, posCam.z]
transformCam = vtk.vtkTransform()
transformCam.SetMatrix(matCam.ravel())
polydata = loadMesh(meshPath, 1)
# Scale STL
transform = vtk.vtkTransform()
# transform.Scale(scale, scale, scale)
# transform.RotateWXYZ(rot[1], rot[2], rot[3], rot[0])
# transform.Translate(pos[0],pos[1], pos[2])
transformFilter = vtk.vtkTransformFilter()
transformFilter.SetTransform(transformCam)
transformFilter.SetInputData(polydata)
transformFilter.Update()
organPolyData = transformFilter.GetOutput()
self._updateActorPolydata(self.actorOrgan, organPolyData)
if self.converter != None:
del (self.converter)
self.converter = uvtoworld.UVToWorldConverter(organPolyData)
self.robot.move(currentPose, interpolate=False)
self.rate.sleep()
measuredPose_current = self.robot.get_current_position()
currentDisplacement = measuredPose_current.p - measuredPose_previous.p
# currentDisplacement = xDotMotion.vel.Norm() * self.resolvedRatesConfig['dt']
currentDisplacement = PyKDL.dot(currentDisplacement,
desiredPose.M.UnitZ())
# currentDisplacement = displacements[len(displacements)-1] + currentDisplacement
forceArray = np.append(forceArray, data, axis=0)
displacements = np.append(displacements, [currentDisplacement])
return displacements.tolist(), forceArray.tolist()
if __name__ == "__main__":
rospy.init_node('probe_2D_server')
yamlFile = cleanResourcePath(
"package://dvrk_vision/defaults/registration_params_cmu.yaml")
with open(yamlFile, 'r') as stream:
data = yaml.load(stream)
cameraTransform = arrayToPyKDLFrame(data['transform'])
np.set_printoptions(precision=2)
# print np.matrix(data['transform'])
# print cameraTransform.M
meshPath = rospy.get_param("~mesh_path")
scale = rospy.get_param("~scale")
objPath = cleanResourcePath(meshPath)
server = Probe2DServer(cameraTransform, objPath, scale)
# TODO turn off embed
embed()
# rospy.spin()
def arrayToPyKDLFrame(array):
rot = arrayToPyKDLRotation(array)
pos = PyKDL.Vector(array[0][3], array[1][3], array[2][3])
return PyKDL.Frame(rot, pos)
if __name__ == "__main__":
import sys
import yaml
import dvrk_vision.vtktools as vtktools
"""A simple example that uses the ForceOverlayWidget class."""
# every QT app needs an app
app = QApplication(['Force Overlay'])
yamlFile = cleanResourcePath(
"package://dvrk_vision/defaults/registration_params.yaml")
with open(yamlFile, 'r') as stream:
data = yaml.load(stream)
cameraTransform = arrayToPyKDLFrame(data['transform'])
rosThread = vtktools.QRosThread()
rosThread.start()
frameRate = 15
slop = 1.0 / frameRate
cams = StereoCameras("stereo/left/image_rect",
"stereo/right/image_rect",
"stereo/left/camera_info",
"stereo/right/camera_info",
slop=slop)
windowL = ForceOverlayWidget(cam=cams.camL,
def __init__(self, visualize=True):
rospy.init_node('stiffness_to_image_converter', anonymous=True)
self.domain = [0, 1, 0, 1]
self.resolution = 100
# self.gp = self.gp_init()
# Publishers and subscribers
self.imagePub = rospy.Publisher('/stereo/stiffness_image',
Image,
queue_size=1)
stiffSub = message_filters.Subscriber('/dvrk/GP/get_stiffness',
Float64MultiArray)
pointsSub = message_filters.Subscriber('/dvrk/GP/get_surface_points',
Float64MultiArray)
stiffSub.registerCallback(self.stiffnessCB)
pointsSub.registerCallback(self.pointsCB)
self.points = np.empty((0, 2))
self.oldPoints = np.empty((0, 3))
self.stiffness = np.empty(0)
self.points2D = np.empty((0, 3))
# Organ stuff
meshPath = "package://oct_15_demo/resources/largeProstate.obj"
texturePath = "package://oct_15_demo/resources/largeProstate.png"
scale = 1000.06
# Read in STL
meshPath = cleanResourcePath(meshPath)
extension = os.path.splitext(meshPath)[1]
if extension == ".stl" or extension == ".STL":
meshReader = vtk.vtkSTLReader()
elif extension == ".obj" or extension == ".OBJ":
meshReader = vtk.vtkOBJReader()
else:
ROS_FATAL("Mesh file has invalid extension (" + extension + ")")
meshReader.SetFileName(meshPath)
# Scale STL
transform = vtk.vtkTransform()
transform.Scale(scale, scale, scale)
transform.Translate(0, .015, -.142)
transform.RotateX(110)
transformFilter = vtk.vtkTransformFilter()
transformFilter.SetTransform(transform)
transformFilter.SetInputConnection(meshReader.GetOutputPort())
transformFilter.Update()
self.organPolydata = transformFilter.GetOutput()
# Set texture to default
self.texture = cv2.imread(cleanResourcePath(texturePath))
self.converter = uvtoworld.UVToWorldConverter(self.organPolydata)
if visualize:
from dvrk_vision.overlay_gui import vtkRosTextureActor
self.visualize = visualize
self.ren = vtk.vtkRenderer()
color = (0, 0, 1)
self.actor_organ = vtkRosTextureActor("stiffness_texture",
color=color)
self.actor_organ.GetProperty().BackfaceCullingOn()
self._updateActorPolydata(self.actor_organ,
polydata=self.organPolydata,
color=color)
self.actor_organ.setTexture(self.texture.copy())
self.actor_organ.textureOnOff(True)
self.ren.AddActor(self.actor_organ)
self.polyData = vtk.vtkPolyData()
# delaunay = vtk.vtkDelaunay2D()
# if vtk.VTK_MAJOR_VERSION <= 5:
# delaunay.SetInput(self.polyData)
# else:
# delaunay.SetInputData(self.polyData)
# mapper = vtk.vtkDataSetMapper()
# mapper.SetInputConnection(delaunay.GetOutputPort())
mapper = vtk.vtkPolyDataMapper()
mapper.SetScalarVisibility(1)
if vtk.VTK_MAJOR_VERSION <= 5:
mapper.SetInput(self.polyData)
else:
mapper.SetInputData(self.polyData)
actor = vtk.vtkActor()
actor.SetMapper(mapper)
actor.GetProperty().SetDiffuse(0)
actor.GetProperty().SetSpecular(0)
actor.GetProperty().SetAmbient(1)
self.ren.AddActor(actor)
self.renWin = vtk.vtkRenderWindow()
self.renWin.AddRenderer(self.ren)
self.iren = vtk.vtkRenderWindowInteractor()
self.iren.SetRenderWindow(self.renWin)
self.renWin.Render()
# cb = vtkTimerCallback(self.renWin)
# cb.update = self.update
# self.iren.AddObserver('TimerEvent', cb.execute)
# self.iren.CreateRepeatingTimer(15)
self.iren.Start()
else:
rate = rospy.Rate(100)
while not rospy.is_shutdown():
self.update()
rate.sleep()