def __init__(self):
# data members, event based
self._robot_name = 'undefined'
self._robot_state = 'uninitialized'
self._robot_state_event = threading.Event()
self._goal_reached = False
self._goal_reached_event = threading.Event()
# continuous publish from dvrk_bridge
self._position_joint_desired = vctDoubleVec()
self._position_cartesian_desired = Pose()
def prepare_cartesian(self):
# make sure the camera is past the cannula and tool vertical
initial_joint_position = self._position_joint_desired
if ((self._robot_name == 'PSM1') or (self._robot_name == 'PSM2') or (self._robot_name == 'PSM3') or (self._robot_name == 'ECM')):
# set in position joint mode
self.set_state_block(state = 'DVRK_POSITION_GOAL_JOINT')
# create a new goal starting with current position
goal = vctDoubleVec()
goal.data[:] = initial_joint_position
goal.data[0] = 0.0
goal.data[1] = 0.0
goal.data[2] = 0.12
self._goal_reached_event.clear()
self.set_position_goal_joint.publish(goal)
self._goal_reached_event.wait(60) # 1 minute at most
if not self._goal_reached:
rospy.signal_shutdown('failed to reach goal')
sys.exit(-1)
def joint_direct(self):
rospy.loginfo(rospy.get_caller_id() + ' -> starting joint direct')
# set in position joint mode
self.set_state_block('DVRK_POSITION_JOINT')
# get current position
initial_joint_position = self._position_joint_desired
rospy.loginfo(rospy.get_caller_id() + " -> testing direct joint position for 2 joints of %i", len(initial_joint_position))
amplitude = math.radians(10.0) # +/- 10 degrees
duration = 5 # seconds
rate = 200 # aiming for 200 Hz
samples = duration * rate
# create a new goal starting with current position
goal = vctDoubleVec()
goal.data[:] = initial_joint_position
for i in xrange(samples):
goal.data[0] = initial_joint_position[0] + amplitude * math.sin(i * math.radians(360.0) / samples)
goal.data[1] = initial_joint_position[1] + amplitude * math.sin(i * math.radians(360.0) / samples)
self.set_position_joint.publish(goal)
rospy.sleep(1.0 / rate)
rospy.loginfo(rospy.get_caller_id() + ' <- joint direct complete')
def joint_goal(self):
rospy.loginfo(rospy.get_caller_id() + ' -> starting joint goal')
# set in position joint mode
self.set_state_block('DVRK_POSITION_GOAL_JOINT')
# get current position
initial_joint_position = self._position_joint_desired
rospy.loginfo(rospy.get_caller_id() + " -> testing goal joint position for 2 joints of %i", len(initial_joint_position))
amplitude = math.radians(10.0)
# create a new goal starting with current position
goal = vctDoubleVec()
goal.data[:] = initial_joint_position
# first motion
goal.data[0] = initial_joint_position[0] + amplitude
goal.data[1] = initial_joint_position[1] - amplitude
self.set_position_goal_joint_publish_and_wait(goal)
# second motion
goal.data[0] = initial_joint_position[0] - amplitude
goal.data[1] = initial_joint_position[1] + amplitude
self.set_position_goal_joint_publish_and_wait(goal)
# back to initial position
goal.data[:] = initial_joint_position
self.set_position_goal_joint_publish_and_wait(goal)
rospy.loginfo(rospy.get_caller_id() + ' <- joint goal complete')
