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')