def __init__(self, arm_name, ros_namespace='SUJ/', expected_interval=1.0): """Constructor. This initializes a few data members.It requires a arm name, this will be used to find the ROS topics for the arm being controlled. For example if the user wants `PSM1`, the ROS topics will be from the namespace `SUJ/PSM1`""" # data members, event based self.__arm_name = arm_name self.__ros_namespace = ros_namespace self.__full_ros_namespace = self.__ros_namespace + self.__arm_name # crtk features self.__crtk_utils = crtk.utils(self, self.__full_ros_namespace, expected_interval) # add crtk features that we need and are supported by the dVRK self.__crtk_utils.add_operating_state() self.__crtk_utils.add_measured_js() self.__crtk_utils.add_measured_cp() self.__crtk_utils.add_move_jp() self.local = self.__Local(self.__full_ros_namespace + '/local', expected_interval) # create node if not rospy.get_node_uri(): rospy.init_node('arm_suj_api_' + self.__arm_name, anonymous=True, log_level=rospy.WARN) else: rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
def configure(self, device_namespace): # ROS initialization if not rospy.get_node_uri(): rospy.init_node('crtk_move_cp_example', anonymous=True, log_level=rospy.WARN) print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) # populate this class with all the ROS topics we need self.crtk_utils = crtk.utils(self, device_namespace) self.crtk_utils.add_operating_state() self.crtk_utils.add_setpoint_cp() self.crtk_utils.add_move_cp()
def configure(self, device_namespace): # ROS initialization if not rospy.get_node_uri(): rospy.init_node('crtk_servo_cp_example', anonymous = True, log_level = rospy.WARN) print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace) # populate this class with all the ROS topics we need self.crtk_utils = crtk.utils(self, device_namespace) self.crtk_utils.add_operating_state() self.crtk_utils.add_setpoint_cp() self.crtk_utils.add_servo_cp() # for all examples self.duration = 10 # 10 seconds self.rate = 200 # aiming for 200 Hz self.samples = self.duration * self.rate
def __init__(self, namespace): self.crtk = crtk.utils(self, namespace) self.crtk.add_setpoint_js() self.crtk.add_servo_jp()
def __init__(self, namespace): self.crtk = crtk.utils(self, namespace) self.crtk.add_operating_state() self.crtk.add_setpoint_cp() self.crtk.add_servo_cp()
def __init__(self, namespace): self.crtk = crtk.utils(self, namespace) self.crtk.add_measured_js()
def __init__(self, namespace): self.crtk = crtk.utils(self, namespace) self.crtk.add_operating_state() self.crtk.add_measured_cp()
def __init__(self, ros_namespace, expected_interval): self.__crtk_utils = crtk.utils(self, ros_namespace, expected_interval) self.__crtk_utils.add_measured_cp()
def __init_arm(self, arm_name, ros_namespace, expected_interval): """Constructor. This initializes a few data members.It requires a arm name, this will be used to find the ROS topics for the arm being controlled. For example if the user wants `PSM1`, the ROS topics will be from the namespace `PSM1`""" # data members, event based self.__arm_name = arm_name self.__ros_namespace = ros_namespace self.__full_ros_namespace = ros_namespace + arm_name # crtk features self.__crtk_utils = crtk.utils(self, self.__full_ros_namespace, expected_interval) # add crtk features that we need and are supported by the dVRK self.__crtk_utils.add_operating_state() self.__crtk_utils.add_setpoint_js() self.__crtk_utils.add_setpoint_cp() self.__crtk_utils.add_measured_js() self.__crtk_utils.add_measured_cp() self.__crtk_utils.add_measured_cv() self.__crtk_utils.add_servo_jp() self.__crtk_utils.add_servo_jr() self.__crtk_utils.add_servo_cp() self.__crtk_utils.add_servo_jf() self.__crtk_utils.add_move_jp() self.__crtk_utils.add_move_jr() self.__crtk_utils.add_move_cp() self.spatial = self.__MeasuredServoCf( self.__full_ros_namespace + '/spatial', expected_interval) self.body = self.__MeasuredServoCf(self.__full_ros_namespace + '/body', expected_interval) self.local = self.__Local(self.__full_ros_namespace + '/local', expected_interval) self.__sub_list = [] self.__pub_list = [] # publishers frame = PyKDL.Frame() self.__body_set_cf_orientation_absolute_pub = rospy.Publisher( self.__full_ros_namespace + '/body/set_cf_orientation_absolute', std_msgs.msg.Bool, latch=True, queue_size=1) self.__use_gravity_compensation_pub = rospy.Publisher( self.__full_ros_namespace + '/use_gravity_compensation', std_msgs.msg.Bool, latch=True, queue_size=1) self.__trajectory_j_set_ratio_pub = rospy.Publisher( self.__full_ros_namespace + '/trajectory_j/set_ratio', std_msgs.msg.Float64, latch=True, queue_size=1) self.__pub_list = [ self.__body_set_cf_orientation_absolute_pub, self.__use_gravity_compensation_pub, self.__trajectory_j_set_ratio_pub ] # subscribers self.__trajectory_j_ratio_sub = rospy.Subscriber( self.__full_ros_namespace + '/trajectory_j/ratio', std_msgs.msg.Float64, self.__trajectory_j_ratio_cb) self.__sub_list = [self.__trajectory_j_ratio_sub] # create node if not rospy.get_node_uri(): rospy.init_node('arm_api', anonymous=True, log_level=rospy.WARN) else: rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
def __init__(self, ros_namespace, expected_interval): self.__crtk_utils = crtk.utils(self, ros_namespace, expected_interval) self.__crtk_utils.add_measured_cf() self.__crtk_utils.add_servo_cf() self.__crtk_utils.add_jacobian()